Kim Youngsik
/
0SAS_FCC_V12
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
0SAS_FCC_V11
by 
Kim Youngsik
Revision 0:a1ad0eb8b619, committed 2018-04-16
- Comitter:
- skyyoungsik
- Date:
- Mon Apr 16 07:16:00 2018 +0000
- Commit message:
- zg
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.cpp Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,133 @@
+#include "HMC5883L.h"
+
+HMC5883L::HMC5883L(PinName sda, PinName scl): i2c(sda, scl)
+{
+ //100KHz, as specified by the datasheet.
+ char rx;
+
+
+ i2c.frequency(400000);
+ //Testar depois com 400KHz
+ //==========================================================================================================
+ // Read chip_id
+ //==========================================================================================================
+ rx = Read(HMC5883L_IDENT_A);
+ if (rx != 0x48)//ID do chip
+ printf("\ninvalid chip id %d\r\n", rx);
+
+ //==========================================================================================================
+ // Let's set the Configuration Register A
+ //==========================================================================================================
+ // This register set's the number of samples averaged per measurement output, the rate at which data is written
+ // to all three data output registers and the measurement flow of the device.
+ // -------------------------------------------------------
+ // |CRA7 CRA6 CRA5 CRA4 CRA3 CRA2 CRA1 CRA0 |
+ // |(1) MA1(1) MA0(1) DO2(1) DO1(0) DO0(0) MS1(0) MS0(0)| -> This is the default value
+ // -------------------------------------------------------
+ // CRA7 -> we have to clear this bit for correct operation (0)
+ // CRA6 to CRA5 -> Let's select the maximum number of samples averaged per measurement output (11)
+ // CRA4 to CRA2 -> Also let's select the maximum data output rate (110)
+ // CRA1 to CRA0 -> The measurement flow is defined to normal (00)
+ // -------------------------------------------------------
+ // |CRA7 CRA6 CRA5 CRA4 CRA3 CRA2 CRA1 CRA0 |
+ // |(0) MA1(1) MA0(1) DO2(1) DO1(1) DO0(0) MS1(0) MS0(0)| -> This is the new value, 0x78 in hex
+ // -------------------------------------------------------
+ //Write(HMC5883L_CONFIG_A,0x78);
+ //Write(HMC5883L_CONFIG_A,0x70);
+
+ //==========================================================================================================
+ // The Configuration Register B is set to 0010 0000 by default, this is a +/- 1.3 Ga sensor field range and
+ // the gain of LSB/gauss is 1090. This is the maximum value, so let's leave it like that.
+ //==========================================================================================================
+ //Datasheet page 13. I will explain later
+ //Write(HMC5883L_CONFIG_B,0x20);
+ //Write(HMC5883L_CONFIG_B,0xA0);
+
+ //==========================================================================================================
+ // Let's set the Mode Register
+ //==========================================================================================================
+ // This register set's the operation mode, from continuous-measurements mode, single-measurement mode and idle mode.
+ // We will set to Continuouse-measurement mode, so the device continuously performs measurements and places the
+ // result in the data register
+ // ---------------------------------------------
+ // |MR7 MR6 MR5 MR4 MR3 MR2 MR1 MR0 | -> This is the new value, 0x78 in hex, we are going to change
+ // |(1) (0) (0) (0) (0) (0) MD1(0) MD0(1)| the MD1 and MD0 to 00 and clear the MR7 for correct operation.
+ // --------------------------------------------- The final value is 0000 0000 (0x00).
+ Write(HMC5883L_MODE,0x00);
+}
+
+
+void HMC5883L::Write(char reg_address, char data)
+{
+ char tx[2];
+ tx[0]=reg_address;
+ tx[1]=data;
+
+ i2c.write(HMC5883L_I2C_WRITE,tx,2);
+}
+
+char HMC5883L::Read(char data)
+{
+ char tx = data;
+ char rx;
+
+ i2c.write(HMC5883L_I2C_WRITE, &tx, 1);
+ i2c.read(HMC5883L_I2C_READ, &rx, 1);
+ return rx;
+}
+
+void HMC5883L::MultiByteRead(char address, char* output, int size)
+{
+ i2c.write(HMC5883L_I2C_WRITE, &address, 1); //tell it where to read from
+ i2c.read(HMC5883L_I2C_READ, output, size); //tell it where to store the data read
+}
+
+float HMC5883L::getMx()
+{
+ //return (x * m_Scale);
+ char lsb_byte = 0;
+ signed short msb_byte;
+
+ lsb_byte = Read(HMC5883L_X_MSB);
+ msb_byte = lsb_byte << 8;
+ msb_byte |= Read(HMC5883L_X_LSB);
+ return (float)msb_byte;
+ /*
+ char tx[1];
+ char rx[2];
+
+
+ tx[0]=HMC5883L_X_MSB;
+ i2c.write(HMC5883L_I2C_READ,tx,1);
+ i2c.read(HMC5883L_I2C_READ,rx,2);
+ return ((int)rx[0]<<8|(int)rx[1]);
+ */
+
+}
+
+float HMC5883L::getMy()
+{
+ //return (y * m_Scale);
+
+ char lsb_byte = 0;
+ signed short msb_byte;
+
+ lsb_byte = Read(HMC5883L_Y_MSB);
+ msb_byte = lsb_byte << 8;
+ msb_byte |= Read(HMC5883L_Y_LSB);
+ return (float)msb_byte;
+}
+
+
+float HMC5883L::getMz()
+{
+ //return (z * m_Scale);
+
+ char lsb_byte = 0;
+ signed short msb_byte;
+
+ lsb_byte = Read(HMC5883L_Z_MSB);
+ msb_byte = lsb_byte << 8;
+ msb_byte |= Read(HMC5883L_Z_LSB);
+ return (float)msb_byte;
+ }
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.h Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,41 @@
+#ifndef HMC5883L_H
+#define HMC5883L_H
+
+#include "mbed.h"
+
+#define HMC5883L_IDENT_A 0x0A // In this case the identification register A is used to identify the devide. ASCII value H
+#define HMC5883L_I2C 0x1E // 7-bit address. 0x3C write, 0x3D read.
+#define HMC5883L_I2C_WRITE 0x3C // Same as (& 0xFE), ensure that the MSB bit is being set to zero (RW=0 -> Writing)
+#define HMC5883L_I2C_READ 0x3D // Same as (| 0x01), ensure that the MSB bit is being set to one (RW=1 -> Reading)
+
+#define HMC5883L_CONFIG_A 0x00
+#define HMC5883L_CONFIG_B 0x01
+#define HMC5883L_MODE 0x02
+#define HMC5883L_STATUS 0x09
+
+#define HMC5883L_X_MSB 0x03
+#define HMC5883L_X_LSB 0x04
+#define HMC5883L_Z_MSB 0x05
+#define HMC5883L_Z_LSB 0x06
+#define HMC5883L_Y_MSB 0x07
+#define HMC5883L_Y_LSB 0x08
+
+
+class HMC5883L
+{
+
+public:
+
+ HMC5883L(PinName sda, PinName scl);
+ float getMx();
+ float getMy();
+ float getMz();
+private:
+ void Write(char reg_address, char data);
+ char Read(char data);
+ void MultiByteRead(char address, char* output, int size);
+ I2C i2c;
+
+};
+
+#endif /* HMC5883L_H */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Mon Apr 16 07:16:00 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/skyyoungsik/code/MPU6050/#f01d2b922a5f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RC_Receiver/ROBOFRIEN_RCV.cpp Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,114 @@
+#include "mbed.h"
+#include "ROBOFRIEN_RCV.h"
+
+static Timer RCV_Timer;
+
+int16_t rcv_channel[6];
+bool rc_state[8];
+/////// RCV 1 //////////////
+int rcv1_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv1_rise(){
+ rcv1_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv1_fall(){
+ rcv_channel[0] = RCV_Timer.read_us() - rcv1_pulse_time;
+ rc_state[0] = true;
+}
+/////// RCV 2 //////////////
+int rcv2_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv2_rise(){
+ rcv2_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv2_fall(){
+ rcv_channel[1] = RCV_Timer.read_us() - rcv2_pulse_time;
+ rc_state[1] = true;
+}
+/////// RCV 3 //////////////
+int rcv3_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv3_rise(){
+ rcv3_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv3_fall(){
+ rcv_channel[2] = RCV_Timer.read_us() - rcv3_pulse_time;
+ rc_state[2] = true;
+}
+/////// RCV 4 //////////////
+int rcv4_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv4_rise(){
+ rcv4_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv4_fall(){
+ rcv_channel[3] = RCV_Timer.read_us() - rcv4_pulse_time;
+ rc_state[3] = true;
+}
+/////// RCV 5 //////////////
+int rcv5_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv5_rise(){
+ rcv5_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv5_fall(){
+ rcv_channel[4] = RCV_Timer.read_us() - rcv5_pulse_time;
+ rc_state[4] = true;
+}
+/////// RCV 6 //////////////
+int rcv6_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv6_rise(){
+ rcv6_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv6_fall(){
+ rcv_channel[5] = RCV_Timer.read_us() - rcv6_pulse_time;
+ rc_state[5] = true;
+}
+
+ROBOFRIEN_RCV::ROBOFRIEN_RCV(PinName RCV1) : _InterruptIn1(RCV1)
+{
+ if(RCV1 == p5){
+ _InterruptIn1.mode(PullDown);
+ _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv1_rise);
+ _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv1_fall);
+ }
+ else if(RCV1 == p6){
+ _InterruptIn1.mode(PullDown);
+ _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv2_rise);
+ _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv2_fall);
+ }
+ else if(RCV1 == p7){
+ _InterruptIn1.mode(PullDown);
+ _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv3_rise);
+ _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv3_fall);
+ }
+ else if(RCV1 == p8){
+ _InterruptIn1.mode(PullDown);
+ _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv4_rise);
+ _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv4_fall);
+ }
+ else if(RCV1 == p11){
+ _InterruptIn1.mode(PullDown);
+ _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv5_rise);
+ _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv5_fall);
+ }
+ else if(RCV1 == p12){
+ _InterruptIn1.mode(PullDown);
+ _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv6_rise);
+ _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv6_fall);
+ }
+}
+void ROBOFRIEN_RCV::Init(){
+ RCV_Timer.start();
+}
+
+///////// LPF ////////////////
+void ROBOFRIEN_RCV::Update(){
+ for(int i=0; i<6; i++){
+ if(rc_state[i] == true){
+ rc_state[i] = false;
+ if(i != 2){
+ lpf_cap[i] = lpf_cap[i] * 0.9 + (rcv_channel[i]/10.0) * 0.1;
+ }else{
+ lpf_cap[i] = lpf_cap[i] * 0.7 + (rcv_channel[i]/10.0) * 0.3;
+ }
+ }
+ }
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RC_Receiver/ROBOFRIEN_RCV.h Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,35 @@
+
+#ifndef MBED_ROBOFRIEN_RCV_H
+#define MBED_ROBOFRIEN_RCV_H
+
+#include "mbed.h"
+
+
+
+class ROBOFRIEN_RCV {
+public:
+ ROBOFRIEN_RCV(PinName RCV1);
+ void Init();
+ bool rc_state[6];
+ float lpf_cap[6];
+ void Update();
+
+
+private:
+ InterruptIn _InterruptIn1;
+
+ void isr_rc_rcv1_rise();
+ void isr_rc_rcv1_fall();
+ void isr_rc_rcv2_rise();
+ void isr_rc_rcv2_fall();
+ void isr_rc_rcv3_rise();
+ void isr_rc_rcv3_fall();
+ void isr_rc_rcv4_rise();
+ void isr_rc_rcv4_fall();
+ void isr_rc_rcv5_rise();
+ void isr_rc_rcv5_fall();
+ void isr_rc_rcv6_rise();
+ void isr_rc_rcv6_fall();
+ };
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,717 @@
+#include "ROBOFRIEN_GUI.h"
+#include "eeprom.h"
+#include "Config.h"
+
+
+Serial pc(USBTX, USBRX);
+
+uint8_t ISR_Modem_ID_Dept, ISR_Modem_ID_Dest;
+uint8_t ISR_Modem_DC_DPN, ISR_Modem_DC_DL;
+uint8_t ISR_Modem_DATA[256];
+uint8_t ISR_Modem_DC_REQUEST;
+uint16_t ISR_Modem_CHKSUM;
+uint8_t pc_isr_cnt;
+uint8_t isr_modem_data_cnt = 0;
+uint8_t pc_parsing_bool = false;
+bool pc_Start_Check;
+uint8_t model_type1;
+uint16_t model_type2;
+uint8_t pc_buffer[256],ex_pc_raw_buffer,pc_raw_buffer[256];
+uint8_t pc_buffer_cnt=0;
+int HomePointChksum,MarkerChksum;
+
+void ROBOFRIEN_GUI::pc_rx_update(){
+ while (pc.readable()){
+ pc_buffer[1] = pc_buffer[0];
+ pc_buffer[0] = pc.getc();
+ // Check SOF //
+ if (( pc_buffer[1] == 254) & (pc_buffer[0] == 254)){
+ pc_Start_Check = true;
+ pc_isr_cnt = 0;
+ }
+ else if(pc_Start_Check == true){
+ if(pc_isr_cnt <= 4){
+ switch(pc_isr_cnt){
+ case 0: ISR_Modem_ID_Dept = pc_buffer[0]; break;
+ case 1: ISR_Modem_ID_Dest = pc_buffer[0]; break;
+ case 2: ISR_Modem_DC_REQUEST = pc_buffer[0]; break;
+ case 3: ISR_Modem_DC_DPN = pc_buffer[0]; break;
+ case 4: ISR_Modem_DC_DL = pc_buffer[0]; isr_modem_data_cnt = 0; break;
+ }
+ }
+ else if( pc_isr_cnt > 4 ){
+ if ((ISR_Modem_DC_DL >= 1) & (pc_isr_cnt <= (4 + ISR_Modem_DC_DL))) {
+ ISR_Modem_DATA[isr_modem_data_cnt] = pc_buffer[0];
+ isr_modem_data_cnt++;
+ }
+ else if(pc_isr_cnt == (4 + ISR_Modem_DC_DL + 2) ){
+ ISR_Modem_CHKSUM = ((uint16_t)pc_buffer[1] * 256 + pc_buffer[0]);
+ }
+ else if(pc_isr_cnt == (4 + ISR_Modem_DC_DL + 2 + 2) ){
+ if((pc_buffer[1] == 255) & (pc_buffer[0] == 255) ){
+ pc_Start_Check = false;
+ pc_parsing_bool = true;
+ }
+ }
+ }
+ pc_isr_cnt ++;
+ }
+ }
+}
+
+uint8_t Modem_ID_Dest;
+uint8_t Modem_ID_Dept;
+uint8_t Modem_DC_DPN;
+uint8_t Modem_DC_DL;
+uint8_t Modem_DATA[256];
+uint8_t Modem_DC_REQUEST;
+
+bool ROBOFRIEN_GUI::rx_bool(){
+ if(pc_parsing_bool == true){
+ pc_parsing_bool = false;
+ int cal_chksum = 0;
+ for(int i=0; i< ISR_Modem_DC_DL; i++){
+ Modem_DATA[i] = ISR_Modem_DATA[i];
+ cal_chksum += ISR_Modem_DATA[i];
+ }
+ if (ISR_Modem_CHKSUM == cal_chksum){
+ return true;
+ }else{
+ return false;
+ }
+ }
+ else{
+ return false;
+ }
+}
+void ROBOFRIEN_GUI::Init(){
+ pc.baud(38400);
+ eeprom_init();
+ /// EEPROM R/W ////
+ // -------- MODEL -------------- //
+ model_type1 = eeprom_read(EEPROM_MODEL_TYPE1);
+ model_type2 = (int)eeprom_read(EEPROM_MODEL_TYPE2_UP) * 256 + eeprom_read(EEPROM_MODEL_TYPE2_DOWN);
+
+ // -------- INPUT - CAP ------------ //
+ cap_min[0] = (int16_t)eeprom_read(EEPROM_RECV_MIN_1) - 127;
+ cap_min[1] = (int16_t)eeprom_read(EEPROM_RECV_MIN_2) - 127;
+ cap_min[2] = (int16_t)eeprom_read(EEPROM_RECV_MIN_3) - 127;
+ cap_min[3] = (int16_t)eeprom_read(EEPROM_RECV_MIN_4) - 127;
+ cap_min[4] = (int16_t)eeprom_read(EEPROM_RECV_MIN_5) - 127;
+ cap_min[5] = (int16_t)eeprom_read(EEPROM_RECV_MIN_6) - 127;
+ cap_min[6] = (int16_t)eeprom_read(EEPROM_RECV_MIN_7) - 127;
+ cap_min[7] = (int16_t)eeprom_read(EEPROM_RECV_MIN_8) - 127;
+
+ cap_neu[0] = (int16_t)eeprom_read(EEPROM_RECV_NEU_1) - 127;
+ cap_neu[1] = (int16_t)eeprom_read(EEPROM_RECV_NEU_2) - 127;
+ cap_neu[2] = (int16_t)eeprom_read(EEPROM_RECV_NEU_3) - 127;
+ cap_neu[3] = (int16_t)eeprom_read(EEPROM_RECV_NEU_4) - 127;
+ cap_neu[4] = (int16_t)eeprom_read(EEPROM_RECV_NEU_5) - 127;
+ cap_neu[5] = (int16_t)eeprom_read(EEPROM_RECV_NEU_6) - 127;
+ cap_neu[6] = (int16_t)eeprom_read(EEPROM_RECV_NEU_7) - 127;
+ cap_neu[7] = (int16_t)eeprom_read(EEPROM_RECV_NEU_8) - 127;
+
+ cap_max[0] = (int16_t)eeprom_read(EEPROM_RECV_MAX_1) - 127;
+ cap_max[1] = (int16_t)eeprom_read(EEPROM_RECV_MAX_2) - 127;
+ cap_max[2] = (int16_t)eeprom_read(EEPROM_RECV_MAX_3) - 127;
+ cap_max[3] = (int16_t)eeprom_read(EEPROM_RECV_MAX_4) - 127;
+ cap_max[4] = (int16_t)eeprom_read(EEPROM_RECV_MAX_5) - 127;
+ cap_max[5] = (int16_t)eeprom_read(EEPROM_RECV_MAX_6) - 127;
+ cap_max[6] = (int16_t)eeprom_read(EEPROM_RECV_MAX_7) - 127;
+ cap_max[7] = (int16_t)eeprom_read(EEPROM_RECV_MAX_8) - 127;
+
+ // --------- OUTPUT - MOTOR ----------- //
+ motor_min[0] = (int)eeprom_read(EEPROM_MOTOR_MIN_1_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_1_DOWN);
+ motor_min[1] = (int)eeprom_read(EEPROM_MOTOR_MIN_2_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_2_DOWN);
+ motor_min[2] = (int)eeprom_read(EEPROM_MOTOR_MIN_3_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_3_DOWN);
+ motor_min[3] = (int)eeprom_read(EEPROM_MOTOR_MIN_4_UP) * 256 + eeprom_read(EEPROM_MOTOR_MIN_4_DOWN);
+
+ // --------- OUTPUT - LED ------------ //
+ headlight_period = eeprom_read(EEPROM_HEADLIGHT_PERIOD);
+ headlight_dutyrate = eeprom_read(EEPROM_HEADLIGHT_DUTYRATE);
+ sidelight_period = eeprom_read(EEPROM_SIDELIGHT_PERIOD);
+ sidelight_dutyrate = eeprom_read(EEPROM_SIDELIGHT_DUTYRATE);
+
+ // --------- GAIN - LIMIT ANGLE ------- //
+ limit_rollx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_ROLL_DOWN) - 32767;
+ limit_pitchx100 = (int)eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_ANGLE_PITCH_DOWN) - 32767;
+ limit_roll_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_ROLL_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_ROLL_DOWN) - 32767;
+ limit_pitch_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_PITCH_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_PITCH_DOWN) - 32767;
+ limit_yaw_rate = (int)eeprom_read(EEPROM_LIMIT_RATE_YAW_UP) * 256 + eeprom_read(EEPROM_LIMIT_RATE_YAW_DOWN) - 32767;
+
+ // --------- GAIN - GAIN DATA --------- //
+ for (int i = 0; i < 20; i++) {
+ gain_px100[i] = (int)eeprom_read(EEPROM_GAIN_P_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_P_DOWN[i]);
+ gain_dx100[i] = (int)eeprom_read(EEPROM_GAIN_D_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_D_DOWN[i]);
+ gain_ix100[i] = (int)eeprom_read(EEPROM_GAIN_I_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_I_DOWN[i]);
+ }
+ cal_accel_bias[0] = (((int)eeprom_read(EEPROM_AHRS_ROLL_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_ROLL_GAP_DOWN)) - 32767)/10000.0;
+ cal_accel_bias[1] = (((int)eeprom_read(EEPROM_AHRS_PITCH_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_PITCH_GAP_DOWN)) - 32767)/10000.0;
+ cal_accel_bias[2] = (((int)eeprom_read(EEPROM_AHRS_YAW_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_YAW_GAP_DOWN)) - 32767)/10000.0;
+
+ // --------- Sensor ----------- ///
+ mag_x_avg = (float)eeprom_read(EEPROM_AHRS_YAW_X_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_X_GAP_2) - 32767 ;
+ mag_y_avg = (float)eeprom_read(EEPROM_AHRS_YAW_Y_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_Y_GAP_2) - 32767 ;
+ mag_z_avg = (float)eeprom_read(EEPROM_AHRS_YAW_Z_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_Z_GAP_2) - 32767 ;
+
+ declination_angle = (float)(((signed long)eeprom_read(EEPROM_AHRS_DECLINATION_ANGLE_UP) * 256 + eeprom_read(EEPROM_AHRS_DECLINATION_ANGLE_DOWN))-18000)/100.0;
+}
+
+void trans_configuration_data(int a, int b, int c);
+void trans_empty_data(int a, int b);
+void trans_flight_data(int a, int b);
+void ROBOFRIEN_GUI::Refresh(){
+ Modem_ID_Dept = ISR_Modem_ID_Dept;
+ Modem_ID_Dest = ISR_Modem_ID_Dest;
+ Modem_DC_DPN = ISR_Modem_DC_DPN;
+ Modem_DC_DL = ISR_Modem_DC_DL;
+ Modem_DC_REQUEST = ISR_Modem_DC_REQUEST;
+ DPN_Info = Modem_DC_DPN;
+ if ((Modem_ID_Dept == 255)&(Modem_ID_Dest == 0)) {
+ if (Modem_DC_REQUEST == 1) {
+ /// REQUST - READ FROM FCS ///
+ if (Modem_DC_DPN == 0) {
+ /// FLIGHT DATA ///
+ if( (Modem_DATA[1] & 0b00000001) == 0b00000001)button[0] = !button[0];
+ if( (Modem_DATA[1] & 0b00000010) == 0b00000010)button[1] = !button[1];
+ if( (Modem_DATA[1] & 0b00000100) == 0b00000100)button[2] = !button[2];
+ if( (Modem_DATA[1] & 0b00001000) == 0b00001000)button[3] = !button[3];
+ if( (Modem_DATA[1] & 0b00010000) == 0b00010000)button[4] = !button[4];
+ // Home Point //
+ if( Modem_DATA[0] == 0){
+ Homepoint_Lat = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
+ Homepoint_Lng = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
+ Homepoint_Alt = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;
+ }else if(Modem_DATA[0] <= 20){
+ Marker_Mode[Modem_DATA[0]-1] = Modem_DATA[2];
+ Marker_Lat[Modem_DATA[0]-1] = (long)Modem_DATA[3] * 16777216 + Modem_DATA[4] * 65536 + Modem_DATA[5] * 256 + Modem_DATA[6] - 90000000;
+ Marker_Lng[Modem_DATA[0]-1] = (long)Modem_DATA[7] * 16777216 + Modem_DATA[8] * 65536 + Modem_DATA[9] * 256 + Modem_DATA[10] - 180000000;
+ Marker_Alt[Modem_DATA[0]-1] = (long)Modem_DATA[11] * 256 + Modem_DATA[12] - 10000;
+ Marker_Speed[Modem_DATA[0]-1] = (long)Modem_DATA[13] * 256 + Modem_DATA[14];
+ }
+ trans_flight_data(TO_GCS, Modem_DC_DPN);
+ }
+ else {
+ trans_configuration_data(TO_GCS, Modem_DC_DPN, Modem_DATA[0]);
+ }
+ }
+ else {
+ /// NOT REQUEST - WRITE TO FCS ///
+ trans_empty_data(TO_GCS, Modem_DC_DPN);
+ switch (Modem_DC_DPN) { // Check DPN //
+ case 0:
+ break;
+ case 1: /// Modem ///
+ model_type1 = Modem_DATA[0];
+ model_type2 = (uint16_t)Modem_DATA[1] * 256 + Modem_DATA[2];
+ eeprom_write(EEPROM_MODEL_TYPE1, (int)model_type1);
+ eeprom_write(EEPROM_MODEL_TYPE2_UP, (int)model_type2 / 256);
+ eeprom_write(EEPROM_MODEL_TYPE2_DOWN, (int)model_type2 % 256);
+ eeprom_refresh();
+ break;
+ case 3: /// RC Receiver ///
+ if ((Modem_DATA[0] >= 1) & (Modem_DATA[0] <= 8)) {
+ cap_min[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[5] - 127;
+ cap_neu[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[6] - 127;
+ cap_max[Modem_DATA[0] - 1] = (int16_t)Modem_DATA[7] - 127;
+ switch (Modem_DATA[0]) {
+ case 1:
+ eeprom_write(EEPROM_RECV_MIN_1, cap_min[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_NEU_1, cap_neu[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_MAX_1, cap_max[Modem_DATA[0] - 1] + 127);
+ break;
+ case 2:
+ eeprom_write(EEPROM_RECV_MIN_2, cap_min[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_NEU_2, cap_neu[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_MAX_2, cap_max[Modem_DATA[0] - 1] + 127);
+ break;
+ case 3:
+ eeprom_write(EEPROM_RECV_MIN_3, cap_min[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_NEU_3, cap_neu[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_MAX_3, cap_max[Modem_DATA[0] - 1] + 127);
+ break;
+ case 4:
+ eeprom_write(EEPROM_RECV_MIN_4, cap_min[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_NEU_4, cap_neu[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_MAX_4, cap_max[Modem_DATA[0] - 1] + 127);
+ break;
+ case 5:
+ eeprom_write(EEPROM_RECV_MIN_5, cap_min[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_NEU_5, cap_neu[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_MAX_5, cap_max[Modem_DATA[0] - 1] + 127);
+ break;
+ case 6:
+ eeprom_write(EEPROM_RECV_MIN_6, cap_min[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_NEU_6, cap_neu[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_MAX_6, cap_max[Modem_DATA[0] - 1] + 127);
+ break;
+ case 7:
+ eeprom_write(EEPROM_RECV_MIN_7, cap_min[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_NEU_7, cap_neu[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_MAX_7, cap_max[Modem_DATA[0] - 1] + 127);
+ break;
+ case 8:
+ eeprom_write(EEPROM_RECV_MIN_8, cap_min[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_NEU_8, cap_neu[Modem_DATA[0] - 1] + 127);
+ eeprom_write(EEPROM_RECV_MAX_8, cap_max[Modem_DATA[0] - 1] + 127);
+ break;
+ }
+ eeprom_refresh();
+ }
+ break;
+ case 4: // Motor - OUTPUT //
+ pwm_info[0] = (int)Modem_DATA[1] * 256 + Modem_DATA[2];
+ pwm_info[1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4];
+ pwm_info[2] = (int)Modem_DATA[5] * 256 + Modem_DATA[6];
+ pwm_info[3] = (int)Modem_DATA[7] * 256 + Modem_DATA[8];
+ switch (Modem_DATA[0]) {
+ case 1:
+ motor_min[0] = pwm_info[0];
+ eeprom_write(EEPROM_MOTOR_MIN_1_UP, motor_min[0] / 256);
+ eeprom_write(EEPROM_MOTOR_MIN_1_DOWN, motor_min[0] % 256);
+ eeprom_refresh();
+ break;
+ case 2:
+ motor_min[1] = pwm_info[1];
+ eeprom_write(EEPROM_MOTOR_MIN_2_UP, motor_min[1] / 256);
+ eeprom_write(EEPROM_MOTOR_MIN_2_DOWN, motor_min[1] % 256);
+ eeprom_refresh();
+ break;
+ case 3:
+ motor_min[2] = pwm_info[2];
+ eeprom_write(EEPROM_MOTOR_MIN_3_UP, motor_min[2] / 256);
+ eeprom_write(EEPROM_MOTOR_MIN_3_DOWN, motor_min[2] % 256);
+ eeprom_refresh();
+ break;
+ case 4:
+ motor_min[3] = pwm_info[3];
+ eeprom_write(EEPROM_MOTOR_MIN_4_UP, motor_min[3] / 256);
+ eeprom_write(EEPROM_MOTOR_MIN_4_DOWN, motor_min[3] % 256);
+ eeprom_refresh();
+ break;
+ }
+ break;
+ case 5: // LED - OUTPUT //
+ headlight_period = Modem_DATA[0];
+ headlight_dutyrate = Modem_DATA[1];
+ sidelight_period = Modem_DATA[2];
+ sidelight_dutyrate = Modem_DATA[3];
+ eeprom_write(EEPROM_HEADLIGHT_PERIOD, headlight_period);
+ eeprom_write(EEPROM_HEADLIGHT_DUTYRATE, headlight_dutyrate);
+ eeprom_write(EEPROM_SIDELIGHT_PERIOD, sidelight_period);
+ eeprom_write(EEPROM_SIDELIGHT_DUTYRATE, sidelight_dutyrate);
+ eeprom_refresh();
+ break;
+ case 6: // AHRS - ROLL , Pitch //
+ /// Attitude ///
+ if (Modem_DATA[0] == 1) {
+ attitude_configuration_bool = true;
+ }
+ else {
+ attitude_configuration_bool = false;
+ }
+ /// Compass ///
+ if (Modem_DATA[7] == 1) {
+ Compass_Calibration_Mode = Modem_DATA[7];
+ }
+ else if (Modem_DATA[7] == 2) {
+ Compass_Calibration_Mode = Modem_DATA[7];
+ }
+ else if(Modem_DATA[7] == 3){
+ Compass_Calibration_Mode = Modem_DATA[7];
+ declination_angle = (float)(((int)Modem_DATA[10] * 256 + Modem_DATA[11]) - 18000)/100.0;
+ eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_UP, (int)((declination_angle+180)*100)/256);
+ eeprom_write(EEPROM_AHRS_DECLINATION_ANGLE_DOWN, (int)((declination_angle+180)*100)%256);
+ eeprom_refresh();
+ }
+ break;
+ case 7: // Limit Angle //
+ limit_rollx100 = ((int)Modem_DATA[0] * 256 + Modem_DATA[1]) - 32767;
+ limit_pitchx100 = ((int)Modem_DATA[2] * 256 + Modem_DATA[3]) - 32767;
+ limit_roll_rate = ((int)Modem_DATA[4] * 256 + Modem_DATA[5]) - 32767;
+ limit_pitch_rate = ((int)Modem_DATA[6] * 256 + Modem_DATA[7]) - 32767;
+ limit_yaw_rate = ((int)Modem_DATA[8] * 256 + Modem_DATA[9]) - 32767;
+ eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_UP, (limit_rollx100 + 32767) / 256);
+ eeprom_write(EEPROM_LIMIT_ANGLE_ROLL_DOWN, (limit_rollx100 + 32767) % 256);
+ eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_UP, (limit_pitchx100 + 32767) / 256);
+ eeprom_write(EEPROM_LIMIT_ANGLE_PITCH_DOWN, (limit_pitchx100 + 32767) % 256);
+ eeprom_write(EEPROM_LIMIT_RATE_ROLL_UP, (limit_roll_rate + 32767) / 256);
+ eeprom_write(EEPROM_LIMIT_RATE_ROLL_DOWN, (limit_roll_rate + 32767) % 256);
+ eeprom_write(EEPROM_LIMIT_RATE_PITCH_UP, (limit_pitch_rate + 32767) / 256);
+ eeprom_write(EEPROM_LIMIT_RATE_PITCH_DOWN, (limit_pitch_rate + 32767) % 256);
+ eeprom_write(EEPROM_LIMIT_RATE_YAW_UP, (limit_yaw_rate + 32767) / 256);
+ eeprom_write(EEPROM_LIMIT_RATE_YAW_DOWN, (limit_yaw_rate + 32767) % 256);
+ eeprom_refresh();
+ break;
+ case 8:
+ int gain_number = Modem_DATA[0];
+ if ((gain_number >= 1) & (gain_number <= 20)) {
+ gain_px100[gain_number - 1] = (int)Modem_DATA[1] * 256 + Modem_DATA[2];
+ gain_dx100[gain_number - 1] = (int)Modem_DATA[3] * 256 + Modem_DATA[4];
+ gain_ix100[gain_number - 1] = (int)Modem_DATA[5] * 256 + Modem_DATA[6];
+ eeprom_write(EEPROM_GAIN_P_UP[gain_number - 1], gain_px100[gain_number - 1] / 256);
+ eeprom_write(EEPROM_GAIN_P_DOWN[gain_number - 1], gain_px100[gain_number - 1] % 256);
+ eeprom_write(EEPROM_GAIN_D_UP[gain_number - 1], gain_dx100[gain_number - 1] / 256);
+ eeprom_write(EEPROM_GAIN_D_DOWN[gain_number - 1], gain_dx100[gain_number - 1] % 256);
+ eeprom_write(EEPROM_GAIN_I_UP[gain_number - 1], gain_ix100[gain_number - 1] / 256);
+ eeprom_write(EEPROM_GAIN_I_DOWN[gain_number - 1], gain_ix100[gain_number - 1] % 256);
+ if(gain_number == 20)eeprom_refresh();
+ }
+ break;
+ }
+
+ }
+ }
+
+}
+void ROBOFRIEN_GUI::trans_empty_data(int id_dest, int data_parse_num) {
+ uint8_t Packet_SOF[2] = { 254,254 };
+ uint8_t Packet_Identifier[2];
+ uint8_t Packet_DC[3];
+ uint8_t Packet_CHKSUM[2];
+ uint8_t Packet_EOF[2] = { 255,255 };
+ // Identifier //
+ Packet_Identifier[0] = 0;
+ Packet_Identifier[1] = id_dest;
+ // Data Control //
+ Packet_DC[0] = 0;
+ Packet_DC[1] = (uint8_t)data_parse_num;
+ int data_length = 0;
+ Packet_DC[2] = (uint8_t)data_length;
+
+ // CHKSUM //
+ unsigned int data_checksum = 0;
+ Packet_CHKSUM[0] = data_checksum / 256;
+ Packet_CHKSUM[1] = data_checksum % 256;
+
+ // Each Buffer Integrate to modem_buffer ///
+ uint8_t modem_buffer[11];
+ modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
+ modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
+ modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
+ modem_buffer[7] = Packet_CHKSUM[0];
+ modem_buffer[8] = Packet_CHKSUM[1];
+ modem_buffer[9] = Packet_EOF[0];
+ modem_buffer[10] = Packet_EOF[1];
+ for (int i = 0; i <= 10; i++) {
+ pc.putc(modem_buffer[i]);
+ }
+}
+
+void ROBOFRIEN_GUI::trans_configuration_data(int id_dest, int data_parse_num, int data_parse_detail_num) {
+
+ uint8_t Packet_SOF[2] = { 254,254 };
+ uint8_t Packet_Identifier[2];
+ uint8_t Packet_DC[3];
+ uint8_t Packet_DATA[16];
+ uint8_t Packet_CHKSUM[2];
+ uint8_t Packet_EOF[2] = { 255,255 };
+ // Identifier //
+ Packet_Identifier[0] = 0;
+ Packet_Identifier[1] = id_dest;
+ // Data Control //
+ Packet_DC[0] = 0;
+ Packet_DC[1] = (uint8_t)data_parse_num;
+ int data_length = 16;
+ Packet_DC[2] = (uint8_t)data_length;
+ // DATA //
+ switch (data_parse_num) {
+ case 0:
+ //// Empty Data ////
+ for (int i = 0; i <= 15; i++) {
+ Packet_DATA[i] = 0;
+ }
+ break;
+ case 1:
+ //// MODEM ////
+ Packet_DATA[0] = model_type1;
+ Packet_DATA[1] = model_type2 / 256; Packet_DATA[2] = model_type2 % 256;
+ for (int i = 3; i <= 15; i++) Packet_DATA[i] = 0;
+ break;
+ case 2:
+ //// Firmware ////
+ for (int i = 0; i <= 13; i++) {
+ Packet_DATA[i] = MODEL_INFO[i];
+ }
+ float firmware_info;
+ firmware_info = FIRMWARE_INFO;
+ Packet_DATA[14] = (uint8_t)(firmware_info * 100 / 256); Packet_DATA[15] = (uint8_t)((uint32_t)(firmware_info * 100) % 256);
+ break;
+ case 3:
+ //// RC Receiver ////
+ if (data_parse_detail_num == 0) {
+ Packet_DATA[0] = data_parse_detail_num; Packet_DATA[1] = 0; Packet_DATA[2] = 0; Packet_DATA[3] = 0; Packet_DATA[4] = 0; Packet_DATA[5] = 0; Packet_DATA[6] = 0; Packet_DATA[7] = 0;
+ }
+ else if(data_parse_detail_num <= 8){
+ Packet_DATA[0] = data_parse_detail_num; Packet_DATA[1] = cap_min[data_parse_detail_num - 1]; Packet_DATA[2] = cap_neu[data_parse_detail_num - 1]; Packet_DATA[3] = cap_max[data_parse_detail_num - 1]; Packet_DATA[4] = 0;
+ Packet_DATA[5] = cap_min[data_parse_detail_num - 1] + 127; Packet_DATA[6] = cap_neu[data_parse_detail_num - 1] + 127; Packet_DATA[7] = cap_max[data_parse_detail_num - 1] + 127;
+ }
+ else {
+ for (int i = 0; i <= 7; i++) Packet_DATA[i] = 0;
+ }
+ Packet_DATA[8] = raw_cap[0] + 127; Packet_DATA[9] = raw_cap[1] + 127; Packet_DATA[10] = raw_cap[2] + 127; Packet_DATA[11] = raw_cap[3] + 127; Packet_DATA[12] = raw_cap[4] + 127; Packet_DATA[13] = raw_cap[5] + 127; Packet_DATA[14] = raw_cap[6] + 127; Packet_DATA[15] = raw_cap[7] + 127;
+ break;
+ case 4:
+ for(int i=0; i<16; i++) Packet_DATA[i] = 0;
+ break;
+ case 5:
+ /////// LED //////////
+ Packet_DATA[0] = headlight_period; Packet_DATA[1] = headlight_dutyrate; Packet_DATA[2] = sidelight_period; Packet_DATA[3] = sidelight_dutyrate;
+ break;
+ case 6:
+ //// AHRS - Roll , Pitch ///
+ Packet_DATA[0] = 0;
+ Packet_DATA[1] = (uint8_t)((unsigned long)(((float)rollx100 / 100.0 + 180) * 100) / 256);
+ Packet_DATA[2] = (uint8_t)((unsigned long)(((float)rollx100 / 100.0 + 180) * 100) % 256);
+ Packet_DATA[3] = (uint8_t)((unsigned long)(((float)pitchx100 / 100.0 + 90) * 100) / 256);
+ Packet_DATA[4] = (uint8_t)((unsigned long)(((float)pitchx100 / 100.0 + 90) * 100) % 256);
+ //// AHRS - Yaw ///
+ Packet_DATA[5] = 0; Packet_DATA[6] = 0;
+ Packet_DATA[7] = (uint8_t)Compass_Calibration_Mode;
+ Packet_DATA[8] = (uint8_t)((unsigned long)(yawx100) / 256);
+ Packet_DATA[9] = (uint8_t)((unsigned long)(yawx100) % 256);
+ Packet_DATA[10] = (uint8_t)((long)((declination_angle + 180) * 100)/256);
+ Packet_DATA[11] = (uint8_t)((long)((declination_angle + 180) * 100)%256);
+ for (int i = 12; i <= 15; i++)Packet_DATA[i] = 0;
+ break;
+ case 7:
+ /// Limit ANGLE ////
+ Packet_DATA[0] = (limit_rollx100 + 32767) / 256;
+ Packet_DATA[1] = (limit_rollx100 + 32767) % 256;
+ Packet_DATA[2] = (limit_pitchx100 + 32767) / 256;
+ Packet_DATA[3] = (limit_pitchx100 + 32767) % 256;
+ Packet_DATA[4] = (limit_roll_rate + 32767) / 256;
+ Packet_DATA[5] = (limit_roll_rate + 32767) % 256;
+ Packet_DATA[6] = (limit_pitch_rate + 32767) / 256;
+ Packet_DATA[7] = (limit_pitch_rate + 32767) % 256;
+ Packet_DATA[8] = (limit_yaw_rate + 32767) / 256;
+ Packet_DATA[9] = (limit_yaw_rate + 32767) % 256;
+ for (int i = 10; i <= 15; i++) Packet_DATA[i] = 0;
+ break;
+ case 8:
+ Packet_DATA[0] = data_parse_detail_num;
+ if ((data_parse_detail_num > 0)&(data_parse_detail_num <= 20)) {
+ Packet_DATA[1] = gain_px100[data_parse_detail_num - 1] / 256;
+ Packet_DATA[2] = gain_px100[data_parse_detail_num - 1] % 256;
+ Packet_DATA[3] = gain_dx100[data_parse_detail_num - 1] / 256;
+ Packet_DATA[4] = gain_dx100[data_parse_detail_num - 1] % 256;
+ Packet_DATA[5] = gain_ix100[data_parse_detail_num - 1] / 256;
+ Packet_DATA[6] = gain_ix100[data_parse_detail_num - 1] % 256;
+ for (int i = 7; i <= 15; i++) Packet_DATA[i] = 0;
+ }
+ else {
+ for (int i = 1; i <= 15; i++) Packet_DATA[i] = 0;
+ }
+ break;
+ }
+
+ // CHKSUM //
+ unsigned int data_checksum = 0;
+ for (int i = 0; i < data_length; i++) {
+ data_checksum += Packet_DATA[i];
+ }
+ Packet_CHKSUM[0] = data_checksum / 256;
+ Packet_CHKSUM[1] = data_checksum % 256;
+
+ // Each Buffer Integrate to modem_buffer ///
+ uint8_t modem_buffer[27];
+ modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
+modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
+modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
+for (int i = 0; i < data_length; i++) {
+ modem_buffer[i + 7] = Packet_DATA[i];
+}
+modem_buffer[23] = Packet_CHKSUM[0];
+modem_buffer[24] = Packet_CHKSUM[1];
+modem_buffer[25] = Packet_EOF[0];
+modem_buffer[26] = Packet_EOF[1];
+for (int i = 0; i <= 26; i++) {
+ pc.putc(modem_buffer[i]);
+}
+}
+
+void ROBOFRIEN_GUI::trans_flight_data(int id_dest, int data_parse_num) {
+
+ uint8_t Packet_SOF[2] = { 254,254 };
+ uint8_t Packet_Identifier[2];
+ uint8_t Packet_DC[3];
+ uint8_t Packet_DATA[73];
+ uint8_t Packet_CHKSUM[2];
+ uint8_t Packet_EOF[2] = { 255,255 };
+ // Identifier //
+ Packet_Identifier[0] = 0;
+ Packet_Identifier[1] = id_dest;
+ // Data Control //
+ Packet_DC[0] = 0;
+ Packet_DC[1] = (uint8_t)data_parse_num;
+ int data_length;
+ if (data_parse_num == 0) data_length = 73;
+ else data_length = 16;
+ Packet_DC[2] = (uint8_t)data_length;
+ // DATA - Modem & State //
+ if(Mode1 >= 15) Mode1 = 15;
+ else if(Mode1 <= 0) Mode1 = 0;
+ if(Mode2 >= 15) Mode2 = 15;
+ else if(Mode2 <= 0) Mode2 = 0;
+ if(MissionState >= 15) MissionState = 15;
+ else if(MissionState <= 0) MissionState = 0;
+ if(CurrentMarker >= 20) CurrentMarker = 20;
+ else if(CurrentMarker <= 0) CurrentMarker = 0;
+ HomePointChksum = ((Homepoint_Lat + 90000000)/16777216) + ((Homepoint_Lat + 90000000)%16777216/65536) + ((Homepoint_Lat + 90000000)%65536/256) + ((Homepoint_Lat + 90000000)%256);
+ HomePointChksum += ((Homepoint_Lng + 180000000)/16777216) + ((Homepoint_Lng + 180000000)%16777216/65536) + ((Homepoint_Lng + 180000000)%65536/256) + ((Homepoint_Lng + 180000000)%256);
+ HomePointChksum += ((Homepoint_Alt + 10000)/256 + (Homepoint_Alt + 10000)%256);
+ MarkerChksum = 0;
+ for(int i=0; i<20; i++){
+ MarkerChksum += Marker_Mode[i];
+ MarkerChksum += (Marker_Lat[i]+90000000)/16777216 + (Marker_Lat[i]+90000000)%16777216/65536 + (Marker_Lat[i]+90000000)%65536/256 + (Marker_Lat[i]+90000000)%256;
+ MarkerChksum += (Marker_Lng[i]+180000000)/16777216 + (Marker_Lng[i]+180000000)%16777216/65536 + (Marker_Lng[i]+180000000)%65536/256 + (Marker_Lng[i]+180000000)%256;
+ MarkerChksum += (Marker_Alt[i]+10000)/256 + (Marker_Alt[i]+10000)%256;
+ MarkerChksum += Marker_Speed[i]/256 + Marker_Speed[i]%256;
+ }
+ Packet_DATA[0] = Mode1 * 16 + Mode2;
+ Packet_DATA[1] = MissionState;
+ Packet_DATA[2] = CurrentMarker;
+ Packet_DATA[3] = Bat1;
+ Packet_DATA[4] = Bat2;
+ Packet_DATA[5] = button[0] * 1 + button[1] * 2 + button[2] * 4 + button[3] * 8 + button[4] * 16;
+ Packet_DATA[6] = HomePointChksum%256;
+ Packet_DATA[7] = MarkerChksum % 256;
+ // DATA - GPS //
+ unsigned long send_gps_time = utc_time;
+ Packet_DATA[8] = send_gps_time / 16777216;
+ Packet_DATA[9] = send_gps_time % 16777216 / 65536;
+ Packet_DATA[10] = send_gps_time % 65536 / 256;
+ Packet_DATA[11] = send_gps_time % 256;
+ signed long send_gps_latitude = latitude + 90000000;
+ Packet_DATA[12] = send_gps_latitude / 16777216;
+ Packet_DATA[13] = send_gps_latitude % 16777216 / 65536;
+ Packet_DATA[14] = send_gps_latitude % 65536 / 256;
+ Packet_DATA[15] = send_gps_latitude % 256;
+ signed long send_gps_longitude = longitude + 180000000;
+ Packet_DATA[16] = send_gps_longitude / 16777216;
+ Packet_DATA[17] = send_gps_longitude % 16777216 / 65536;
+ Packet_DATA[18] = send_gps_longitude % 65536 / 256;
+ Packet_DATA[19] = send_gps_longitude % 256;
+ signed long send_altitude;
+ if ((altitude + 10000) < 0) send_altitude = -10000;
+ else send_altitude = (altitude + 10000);
+ Packet_DATA[20] = send_altitude / 256;
+ Packet_DATA[21] = send_altitude % 256;
+ Packet_DATA[22] = SatNum;
+ // AHRS //
+ unsigned long send_roll = rollx100 + 32767;
+ Packet_DATA[23] = send_roll / 256;
+ Packet_DATA[24] = send_roll % 256;
+ unsigned long send_pitch = pitchx100 + 32767;
+ Packet_DATA[25] = send_pitch / 256;
+ Packet_DATA[26] = send_pitch % 256;
+ unsigned long send_yaw = yawx100;
+ Packet_DATA[27] = send_yaw / 256;
+ Packet_DATA[28] = send_yaw % 256;
+ unsigned long send_roll_rate = (roll_ratex100 +32767);
+ Packet_DATA[29] = send_roll_rate / 256;
+ Packet_DATA[30] = send_roll_rate % 256;
+ unsigned long send_pitch_rate = (pitch_ratex100 + 32767);
+ Packet_DATA[31] = send_pitch_rate / 256;
+ Packet_DATA[32] = send_pitch_rate % 256;
+ unsigned long send_yaw_rate = (yaw_ratex100 + 32767);
+ Packet_DATA[33] = send_yaw_rate / 256;
+ Packet_DATA[34] = send_yaw_rate % 256;
+ unsigned long send_velocity_x = (VXx100 + 32767);
+ Packet_DATA[35] = send_velocity_x / 256;
+ Packet_DATA[36] = send_velocity_x % 256;
+ unsigned long send_velocity_y = (VYx100 + 32767);
+ Packet_DATA[37] = send_velocity_y / 256;
+ Packet_DATA[38] = send_velocity_y % 256;
+ unsigned long send_velocity_z = (VZx100 + 32767);
+ Packet_DATA[39] = send_velocity_z / 256;
+ Packet_DATA[40] = send_velocity_z % 256;
+ // CAP and PWM //
+ Packet_DATA[41] = (cap[0] + 127);
+ Packet_DATA[42] = (cap[1] + 127);
+ Packet_DATA[43] = (cap[2] + 127);
+ Packet_DATA[44] = (cap[3] + 127);
+ Packet_DATA[45] = (cap[4] + 127);
+ Packet_DATA[46] = (cap[5] + 127);
+ Packet_DATA[47] = (cap[6] + 127);
+ Packet_DATA[48] = (cap[7] + 127);
+
+ // PWM //
+ for (int i = 49; i <= 56; i++) {
+ Packet_DATA[i] = pwm[i-49];
+ }
+ // DEBUG //
+ Packet_DATA[57] = (int)((DEBUGx100[0]))/256;
+ Packet_DATA[58] = (int)((DEBUGx100[0]))%256;
+ Packet_DATA[59] = (int)((DEBUGx100[1]))/256;
+ Packet_DATA[60] = (int)((DEBUGx100[1]))%256;
+ Packet_DATA[61] = (int)((DEBUGx100[2]))/256;
+ Packet_DATA[62] = (int)((DEBUGx100[2]))%256;
+ Packet_DATA[63] = (int)((DEBUGx100[3]))/256;
+ Packet_DATA[64] = (int)((DEBUGx100[3]))%256;
+ Packet_DATA[65] = (int)((DEBUGx100[4]))/256;
+ Packet_DATA[66] = (int)((DEBUGx100[4]))%256;
+ Packet_DATA[67] = (int)((DEBUGx100[5]))/256;
+ Packet_DATA[68] = (int)((DEBUGx100[5]))%256;
+ Packet_DATA[69] = (int)((DEBUGx100[6]))/256;
+ Packet_DATA[70] = (int)((DEBUGx100[6]))%256;
+ Packet_DATA[71] = (int)((DEBUGx100[7]))/256;
+ Packet_DATA[72] = (int)((DEBUGx100[7]))%256;
+ // CHKSUM //
+ unsigned int data_checksum = 0;
+ for (int i = 0; i < data_length; i++) {
+ data_checksum += Packet_DATA[i];
+ }
+ Packet_CHKSUM[0] = data_checksum / 256;
+ Packet_CHKSUM[1] = data_checksum % 256;
+
+ // Each Buffer Integrate to modem_buffer ///
+ uint8_t modem_buffer[84];
+ modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
+ modem_buffer[2] = Packet_Identifier[0]; modem_buffer[3] = Packet_Identifier[1];
+ modem_buffer[4] = Packet_DC[0]; modem_buffer[5] = Packet_DC[1]; modem_buffer[6] = Packet_DC[2];
+ for (int i = 0; i < 73; i++) {
+ modem_buffer[i + 7] = Packet_DATA[i];
+ }
+ modem_buffer[80] = Packet_CHKSUM[0];
+ modem_buffer[81] = Packet_CHKSUM[1];
+ modem_buffer[82] = Packet_EOF[0];
+ modem_buffer[83] = Packet_EOF[1];
+ for (int i = 0; i <= 83; i++) {
+ pc.putc(modem_buffer[i]);
+ }
+}
+
+void ROBOFRIEN_GUI::attitude_calibrate(float in_acc_bias_x, float in_acc_bias_y, float in_acc_bias_z){
+// calibrate_gap_roll += rollAngle;
+// calibrate_gap_pitch += pitchAngle;
+ cal_accel_bias[0] = in_acc_bias_x;
+ cal_accel_bias[1] = in_acc_bias_y;
+ cal_accel_bias[2] = (in_acc_bias_z);
+ eeprom_write(EEPROM_AHRS_ROLL_GAP_UP, (((unsigned long)(in_acc_bias_x * 10000 + 32767) / 256)));
+ eeprom_write(EEPROM_AHRS_ROLL_GAP_DOWN, (((unsigned long)(in_acc_bias_x * 10000 + 32767) % 256)));
+ eeprom_write(EEPROM_AHRS_PITCH_GAP_UP, (((unsigned long)(in_acc_bias_y * 10000 + 32767) / 256)));
+ eeprom_write(EEPROM_AHRS_PITCH_GAP_DOWN, (((unsigned long)(in_acc_bias_y * 10000 + 32767) % 256)));
+ eeprom_write(EEPROM_AHRS_YAW_GAP_UP, (((unsigned long)(in_acc_bias_z * 10000 + 32767) / 256)));
+ eeprom_write(EEPROM_AHRS_YAW_GAP_DOWN, (((unsigned long)(in_acc_bias_z * 10000 + 32767) % 256)));
+ eeprom_refresh();
+}
+
+void ROBOFRIEN_GUI::write_compass_setting_to_eeprom(){
+ eeprom_write(EEPROM_AHRS_YAW_X_GAP_1, ( (uint32_t)(mag_x_avg + 32767)/256 ) );
+ eeprom_write(EEPROM_AHRS_YAW_X_GAP_2, ( (uint32_t)(mag_x_avg + 32767)%256 ) );
+// eeprom_write(EEPROM_AHRS_YAW_X_GAP_FLOAT, ( (unsigned long)(mag_x_avg * 100)%100 ) );
+// eeprom_write(EEPROM_AHRS_YAW_X_GAP_3, ( (uint32_t)(mag_x_avg * 100 + 2147483648)%65536/256 ) );
+// eeprom_write(EEPROM_AHRS_YAW_X_GAP_4, ( (uint32_t)(mag_x_avg * 100 + 2147483648)%256 ) );
+
+ eeprom_write(EEPROM_AHRS_YAW_Y_GAP_1, ( (uint32_t)(mag_y_avg + 32767)/256 ) );
+ eeprom_write(EEPROM_AHRS_YAW_Y_GAP_2, ( (uint32_t)(mag_y_avg + 32767)%256 ) );
+// eeprom_write(EEPROM_AHRS_YAW_Y_GAP_FLOAT, ( (unsigned long)(mag_y_avg * 100)%100 ) );
+// eeprom_write(EEPROM_AHRS_YAW_Y_GAP_3, ( (uint32_t)(mag_y_avg * 100 + 2147483648)%65536/256 ) );
+// eeprom_write(EEPROM_AHRS_YAW_Y_GAP_4, ( (uint32_t)(mag_y_avg * 100 + 2147483648)%256 ) );
+
+ eeprom_write(EEPROM_AHRS_YAW_Z_GAP_1, ( (uint32_t)(mag_z_avg + 32767)/256 ) );
+ eeprom_write(EEPROM_AHRS_YAW_Z_GAP_2, ( (uint32_t)(mag_z_avg + 32767)%256 ) );
+
+ eeprom_refresh();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_GUI/ROBOFRIEN_GUI.h Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,79 @@
+
+#ifndef MBED_ROBOFRIEN_GUI_H
+#define MBED_ROBOFRIEN_GUI_H
+
+#include "mbed.h"
+
+
+
+class ROBOFRIEN_GUI {
+public:
+ void pc_rx_update();
+ bool rx_bool();
+ void Init();
+ void Refresh();
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////// [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];
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////// [M/S] Mode and State ///////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+ int8_t Mode1, Mode2,MissionState,CurrentMarker;
+ uint8_t Bat1,Bat2;
+ bool button[5];
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////// [GPS] GPS //////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+ unsigned long utc_time;
+ signed long latitude,longitude,altitude;
+ uint8_t SatNum;
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////// AHRS //////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+ int32_t rollx100, pitchx100, yawx100;
+ int32_t roll_ratex100, pitch_ratex100, yaw_ratex100;
+ int32_t VXx100,VYx100,VZx100;
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////// [C/P] CAP/PWM ////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+ int16_t cap[8];
+ uint8_t pwm[8];
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////// [E/D] Extra & Debug /////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+ uint16_t DEBUGx100[8];
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////// Configuration ///////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////
+ 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];
+ 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;
+ 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];
+ void attitude_calibrate(float , float , float);
+ void write_compass_setting_to_eeprom();
+private:
+ void trans_configuration_data(int id_dest, int data_parse_num, int data_parse_detail_num);
+ void trans_flight_data(int id_dest, int data_parse_num);
+ void trans_empty_data(int id_dest, int data_parse_num);
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/Config.h Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,119 @@
+#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/ROBOFRIEN_EXT_UART.cpp Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,131 @@
+#include "ROBOFRIEN_EXT_UART.h"
+
+static Serial EXT_UART(p28,p27);
+
+void ROBOFRIEN_EXT_UART::Init(){
+ EXT_UART.baud(38400);
+}
+
+void ROBOFRIEN_EXT_UART::Refresh(){
+
+
+ uint8_t Packet_SOF[2] = { 254,254 };
+ uint8_t Packet_DATA[32];
+ uint8_t Packet_CHKSUM[2];
+ uint8_t Packet_EOF[2] = { 255,255 };
+
+ // AHRS //
+ unsigned long send_roll = roll*100 + 32767;
+ Packet_DATA[0] = send_roll / 256;
+ Packet_DATA[1] = send_roll % 256;
+ unsigned long send_pitch = pitch*100 + 32767;
+ Packet_DATA[2] = send_pitch / 256;
+ Packet_DATA[3] = send_pitch % 256;
+ unsigned long send_yaw = yaw*100;
+ Packet_DATA[4] = send_yaw / 256;
+ Packet_DATA[5] = send_yaw % 256;
+ unsigned long send_want_yaw = want_yaw*100;
+ Packet_DATA[6] = send_want_yaw / 256;
+ Packet_DATA[7] = send_want_yaw % 256;
+
+ // CAP and PWM //
+ Packet_DATA[8] = (cap[0] + 127);
+ Packet_DATA[9] = (cap[1] + 127);
+ Packet_DATA[10] = (cap[2] + 127);
+ Packet_DATA[11] = (cap[3] + 127);
+ Packet_DATA[12] = (cap[4] + 127);
+ Packet_DATA[13] = (cap[5] + 127);
+
+ Packet_DATA[14] = pwm[0];
+ Packet_DATA[15] = pwm[1];
+ Packet_DATA[16] = pwm[2];
+ Packet_DATA[17] = pwm[3];
+ Packet_DATA[18] = pwm[4];
+ Packet_DATA[19] = pwm[5];
+ Packet_DATA[20] = pwm[6];
+ Packet_DATA[21] = pwm[7];
+
+ Packet_DATA[22] = debug[0];
+ Packet_DATA[23] = debug[1];
+ Packet_DATA[24] = debug[2];
+ Packet_DATA[25] = debug[3];
+ Packet_DATA[26] = debug[4];
+ Packet_DATA[27] = debug[5];
+ Packet_DATA[28] = debug[6];
+ Packet_DATA[29] = debug[7];
+
+ // CHKSUM //
+ unsigned int data_checksum = 0;
+ for (int i = 0; i < 30; i++) {
+ data_checksum += Packet_DATA[i];
+ }
+ Packet_CHKSUM[0] = data_checksum / 256;
+ Packet_CHKSUM[1] = data_checksum % 256;
+
+ // Each Buffer Integrate to modem_buffer ///
+ modem_buffer[0] = Packet_SOF[0]; modem_buffer[1] = Packet_SOF[1];
+ for (int i = 0; i < 30; i++) {
+ modem_buffer[i + 2] = Packet_DATA[i];
+ }
+ modem_buffer[32] = Packet_CHKSUM[0];
+ modem_buffer[33] = Packet_CHKSUM[1];
+ modem_buffer[34] = Packet_EOF[0];
+ modem_buffer[35] = Packet_EOF[1];
+
+ //// TRANS START ////
+ trans_cnt = 0;
+}
+
+
+void ROBOFRIEN_EXT_UART::Trans(){
+
+ if(trans_cnt > 35){
+
+ }else{
+ if(EXT_UART.writeable()){
+ EXT_UART.putc(modem_buffer[trans_cnt]);
+ trans_cnt ++;
+ }
+ }
+}
+
+char uart_ID[3],uart_buffer[256];
+bool uart_start_check,uart_parsing_bool;
+char uart_isr_cnt;
+void ROBOFRIEN_EXT_UART::Receive(){
+ while (EXT_UART.readable()){
+ uart_ID[1] = uart_ID[0];
+ uart_ID[0] = EXT_UART.getc();
+ // Check SOF //
+ if ( (uart_parsing_bool == false) & ( uart_ID[1] == 254) & (uart_ID[0] == 254)){
+ uart_start_check = true;
+ uart_isr_cnt = 0;
+ }
+ else if(uart_start_check == true){
+ uart_buffer[uart_isr_cnt] = uart_ID[0];
+ if(uart_isr_cnt == 7){
+ if( (uart_ID[0] == 255) & (uart_ID[1] == 255) ){
+ uart_parsing_bool = true;
+ }else{
+ uart_parsing_bool = false;
+ }
+ uart_start_check = false;
+ }
+ uart_isr_cnt ++;
+ }
+ }
+ /// Uart Parsing Bool ///
+ if(uart_parsing_bool == true){
+ uart_parsing_bool = false;
+ int cal_chksum;
+ for(int i=0; i<4; i++){
+ cal_chksum += uart_buffer[i];
+ }
+// if(cal_chksum == ( (unsigned int)uart_buffer[4] * 256 + uart_buffer[5] ) ){
+ uart_stick_cap[0] = (signed int)uart_buffer[0]-127;
+ uart_stick_cap[1] = (signed int)uart_buffer[1]-127;
+ uart_stick_cap[2] = (signed int)uart_buffer[2]-127;
+ uart_stick_cap[3] = (signed int)uart_buffer[3]-127;
+// }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_EXT_UART.h Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,24 @@
+
+#ifndef MBED_ROBOFRIEN_EXT_UART_H
+#define MBED_ROBOFRIEN_EXT_UART_H
+
+#include "mbed.h"
+
+
+
+class ROBOFRIEN_EXT_UART {
+public:
+ void Init();
+ void Refresh();
+ void Trans();
+ void Receive();
+
+ float roll,pitch,yaw,want_yaw;
+ signed int cap[8],pwm[8],debug[8];
+ signed int uart_stick_cap[6];
+private:
+ uint8_t modem_buffer[36];
+ uint8_t trans_cnt;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_LED.cpp Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,51 @@
+#include "ROBOFRIEN_LED.h"
+
+Timer LedTimer;
+DigitalOut myled1(LED1);
+DigitalOut myled4(LED4);
+
+void ROBOFRIEN_LED::Init(){
+ LedTimer.start();
+ myled1 = 0;
+ myled4 = 0;
+}
+
+int ex_headlight_timer, ex_sidelight_timer;
+void ROBOFRIEN_LED::Update(){
+ //// LED - HEAD LIGHT ///
+ if( (HeadlightPeriod == 0) | (HeadlightDutyrate == 0) ){
+ myled1 = 0;
+ }else{
+ if( (ex_headlight_timer + ((int)HeadlightPeriod * 100) ) >= LedTimer.read_ms() ){
+ if( (ex_headlight_timer + (float)((int)HeadlightPeriod * 100) * ((float)HeadlightDutyrate / 100.0) ) >= LedTimer.read_ms() ){
+ myled1 = 1;
+ }else{
+ myled1 = 0;
+ }
+ }else{
+ ex_headlight_timer = LedTimer.read_ms();
+ }
+ }
+ //// LED - SIDE LIGHT ///
+ if( (SidelightPeriod == 0) | (SidelightDutyrate == 0) ){
+ myled4 = 0;
+ }else{
+ if( (ex_sidelight_timer + ((int)SidelightPeriod * 100) ) >= LedTimer.read_ms() ){
+ if( (ex_sidelight_timer + (float)((int)SidelightPeriod * 100) * ((float)SidelightDutyrate / 100.0) ) >= LedTimer.read_ms() ){
+ myled4 = 1;
+ }else{
+ myled4 = 0;
+ }
+ }else{
+ ex_sidelight_timer = LedTimer.read_ms();
+ }
+ }
+
+ /////////// RESET //////////////
+ if( LedTimer.read_ms() > 1000000000){
+ ex_headlight_timer = 0;
+ ex_sidelight_timer = 0;
+ LedTimer.reset();
+ }
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_LED.h Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,19 @@
+
+#ifndef MBED_ROBOFRIEN_LED_H
+#define MBED_ROBOFRIEN_LED_H
+
+#include "mbed.h"
+
+
+
+class ROBOFRIEN_LED {
+public:
+ void Init();
+ void Update();
+
+ uint8_t HeadlightPeriod,HeadlightDutyrate,SidelightPeriod,SidelightDutyrate;
+private:
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ROBOFRIEN_SUB_FUNC/eeprom.h Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,50 @@
+#define eeprom_length 256
+
+
+LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+
+volatile int eeprom_address[eeprom_length],eeprom_data[eeprom_length];
+void eeprom_init(){
+ 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);
+ }
+ 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]);
+ }
+ }
+ fclose(file);
+}
+
+void eeprom_refresh(){
+ /// Write Data to EEPROM //
+ 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);
+}
+
+void eeprom_write(int addr, int data){
+ /// Change Data //
+ eeprom_data[addr] = data;
+}
+
+
+int eeprom_read(int addr){
+ return eeprom_data[addr];
+
+}
+
+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);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,711 @@
+
+////// 0SAS_FCC_V6 : Barometer 부착
+////// 0SAS_FCC_V7 : Barometer 제거 / External UART Output 추가
+////// 0SAS_FCC_V8 : PWM 최대 최소 추가, 초기 가속도 측정 wait 시간 늘리기
+
+#include "mbed.h"
+#include "MPU6050.h"
+#include <math.h>
+#include "ROBOFRIEN_RCV.h"
+#include "ROBOFRIEN_GUI.h"
+#include "ROBOFRIEN_LED.h"
+#include "ROBOFRIEN_EXT_UART.h"
+
+//#define yaw_rate_control
+
+#define pwm_min 0.439
+#define pwm_max 0.927 // pwm_min = 0.9ms, pwm_max = 1.9ms
+
+#ifdef yaw_rate_control
+#else
+ #include "HMC5883L.h"
+ HMC5883L hmc5883l(p9, p10);
+#endif
+
+ROBOFRIEN_EXT_UART EXT_UART;
+ROBOFRIEN_GUI GUI;
+ROBOFRIEN_LED LED;
+ROBOFRIEN_RCV RCV1(p5);
+ROBOFRIEN_RCV RCV2(p6);
+ROBOFRIEN_RCV RCV3(p7);
+ROBOFRIEN_RCV RCV4(p8);
+ROBOFRIEN_RCV RCV5(p11);
+ROBOFRIEN_RCV RCV6(p12);
+MPU6050 mpu6050;
+
+
+PwmOut PWM1(p26);
+PwmOut PWM2(p25);
+PwmOut PWM3(p24);
+PwmOut PWM4(p23);
+PwmOut PWM5(p22);
+PwmOut PWM6(p21);
+Ticker timer_500hz;
+
+Timer MainTimer;
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+void compFilter();
+void rc_cap_calibrate();
+/// Setting data from Data Modem ////
+int16_t limit_integral_rate_roll,limit_integral_rate_pitch,limit_integral_rate_yaw;
+
+///////////////////////////////////////
+float x, y, z, heading;
+float max_m_x = 0,min_m_x = 0,max_m_y = 0,min_m_y = 0,max_m_z = 0,min_m_z = 0;
+float pitchAngle = 0,rollAngle = 0,yawAngle = 0;
+int16_t gyro_data[3];
+float roll_rate,pitch_rate,yaw_rate;
+float want_yaw;
+void get_ahrs_data();
+float get_compass_heading();
+float complementary_roll,complementary_pitch,complementary_yaw;
+float raw_mag_x, raw_mag_y, raw_mag_z;
+void compass_init(){
+#ifdef yaw_rate_control
+
+#else
+ raw_mag_x = hmc5883l.getMx();
+ raw_mag_y = -hmc5883l.getMy();
+ raw_mag_z = -hmc5883l.getMz();
+ want_yaw = get_compass_heading();
+ complementary_yaw = want_yaw;
+#endif
+}
+void ahrs_init(){
+ mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
+ mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
+ mpu6050.init(); // Initialize the sensor
+// timer_500hz.attach(&compFilter, 0.002); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
+ wait(0.5);
+}
+
+/* This function is created to avoid address error that caused from Ticker.attach func */
+void SAS_Algorithm();
+int gap_time,current_ahrs_time,ex_ahrs_time;
+bool ahrs_rcv_bool;
+void compFilter() {
+ mpu6050.readAccelData(accelData);
+ mpu6050.readGyroData(gyroData);
+#ifdef yaw_rate_control
+
+#else
+ raw_mag_x = hmc5883l.getMx();
+ raw_mag_y = -hmc5883l.getMy();
+ raw_mag_z = -hmc5883l.getMz();
+#endif
+ ex_ahrs_time = current_ahrs_time;
+ current_ahrs_time = MainTimer.read_us();
+ gap_time = current_ahrs_time - ex_ahrs_time;
+ get_ahrs_data();
+ ahrs_rcv_bool = true;
+}
+float get_compass_heading(){
+ float tilt_heading,no_tilt_heading;
+ x = ( raw_mag_x - GUI.mag_x_avg )*0.92;
+ y = ( raw_mag_y - GUI.mag_y_avg )*0.92;
+ z = ( raw_mag_z - GUI.mag_z_avg )*0.92;
+
+ //// TILT COMPENSATION /////
+ float cosRoll = cos(rollAngle*PI/180);
+ float sinRoll = sin(rollAngle*PI/180);
+ float cosPitch = cos(pitchAngle*PI/180);
+ float sinPitch = sin(pitchAngle*PI/180);
+
+ float Xh = x * cosRoll + y * sinPitch * sinRoll - z * cosPitch * sinRoll;
+ float Yh = ( y * cosPitch + z * sinRoll );
+// float heading = atan2(y,x);
+
+// heading = atan2(Yh, Xh) - (PI/2) + declination_angle*PI/180;
+// heading = atan2(Yh, Xh) + declination_angle*PI/180;
+// heading = -atan2(y,x);
+// heading = atan2(Yh,Xh);
+// heading = atan2(y,x) + declination_angle * PI/180;
+ tilt_heading = atan2(Yh,Xh) - PI / 2 + GUI.declination_angle * PI/180;
+ no_tilt_heading = (atan2(y,x) - PI / 2 + GUI.declination_angle * PI/180);
+
+ if(tilt_heading < 0)
+ tilt_heading += 2*PI;
+ if(tilt_heading > 2*PI)
+ tilt_heading -= 2*PI;
+
+ tilt_heading = tilt_heading * 180 / PI;
+
+ if(no_tilt_heading < 0)
+ no_tilt_heading += 2*PI;
+ if(no_tilt_heading > 2*PI)
+ no_tilt_heading -= 2*PI;
+
+ no_tilt_heading = no_tilt_heading * 180 / PI;
+
+ return no_tilt_heading;
+}
+void magnetic_offset_reset(){
+ max_m_x = raw_mag_x;
+ min_m_x = raw_mag_x;
+ max_m_y = raw_mag_y;
+ min_m_y = raw_mag_y;
+ max_m_z = raw_mag_z;
+ min_m_z = raw_mag_z;
+ GUI.mag_x_avg = (max_m_x + min_m_x)/2.0;
+ GUI.mag_y_avg = (max_m_y + min_m_y)/2.0;
+ GUI.mag_z_avg = (max_m_z + min_m_z)/2.0;
+}
+void magnetic_calibration(){
+ /// compass min / max check //
+ if( raw_mag_x > max_m_x) max_m_x = raw_mag_x;
+ if( raw_mag_x < min_m_x) min_m_x = raw_mag_x;
+ if( raw_mag_y > max_m_y) max_m_y = raw_mag_y;
+ if( raw_mag_y < min_m_y) min_m_y = raw_mag_y;
+ if( raw_mag_z > max_m_z) max_m_z = raw_mag_z;
+ if( raw_mag_z < min_m_z) min_m_z = raw_mag_z;
+ GUI.mag_x_avg = (max_m_x + min_m_x)/2.0;
+ GUI.mag_y_avg = (max_m_y + min_m_y)/2.0;
+ GUI.mag_z_avg = (max_m_z + min_m_z)/2.0;
+}
+float ahrs_lpf_alpha;
+void get_ahrs_data(){
+// mpu6050.complementaryFilter(&rollAngle, &pitchAngle);
+ /* Get actual acc value */
+ mpu6050.getAres();
+ ax = accelData[0]*aRes - GUI.cal_accel_bias[0];
+ ay = accelData[1]*aRes - GUI.cal_accel_bias[1];
+ az = accelData[2]*aRes - GUI.cal_accel_bias[2];
+
+ /* Get actual gyro value */
+ mpu6050.getGres();
+ roll_rate = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i]
+ pitch_rate = -gyroData[1]*gRes; // - gyroBias[1];
+ yaw_rate = -gyroData[2]*gRes; // - gyroBias[2];
+
+ /* Integrate the gyro data(deg/s) over time to get angle */
+ complementary_roll += roll_rate * (float)gap_time/1000000.0; // Angle around the X-axis
+ complementary_pitch += pitch_rate * (float)gap_time/1000000.0; // Angle around the Y-axis
+ complementary_yaw += yaw_rate * (float)gap_time/1000000.0; // Angle around the Z-axis
+
+ float pitchAcc, rollAcc;
+// rollAcc = atan2f(accelData[1], accelData[2])*180/PI;
+// pitchAcc = atan2f(accelData[0], accelData[2])*180/PI;
+ rollAcc = atan2f(ay, az)*180/PI;
+ pitchAcc = atan2f(ax, az)*180/PI;
+/*
+ float forceMagnitudeApprox = sqrt( (ax*ax) + (ay*ay) + (az*az) );
+ float accelerometer_factor = (2 - forceMagnitudeApprox); // At 2G accelerometer_factor = 0 // Range = 0.0 ~ 1.0
+ if(accelerometer_factor >= 1) accelerometer_factor = 1;
+ else if(accelerometer_factor <= 0) accelerometer_factor = 0;
+ rollAngle = rollAngle * ( 1 - accelerometer_factor / 50.0 ) + rollAcc * (accelerometer_factor / 50.0);
+ pitchAngle = pitchAngle * ( 1 - accelerometer_factor / 50.0 ) + pitchAcc * (accelerometer_factor / 50.0);
+*/
+ complementary_roll = complementary_roll * ahrs_lpf_alpha + rollAcc * (1-ahrs_lpf_alpha);
+ complementary_pitch = complementary_pitch * ahrs_lpf_alpha + pitchAcc * (1-ahrs_lpf_alpha);
+
+ rollAngle = complementary_roll;// - GUI.calibrate_gap_roll;
+ pitchAngle = complementary_pitch;// - GUI.calibrate_gap_pitch;
+
+#ifdef yaw_rate_control
+
+#else
+ /// CAL COMPASS YAW ////
+ float data_yaw_to_180;
+ data_yaw_to_180 = 180 - complementary_yaw;
+
+ complementary_yaw = 180;
+ float tmp_compass_heading;
+ tmp_compass_heading = get_compass_heading() + data_yaw_to_180;
+ if (tmp_compass_heading > 360) tmp_compass_heading -= 360;
+ if (tmp_compass_heading < 0) tmp_compass_heading += 360;
+ complementary_yaw = complementary_yaw*0.999 + tmp_compass_heading*0.001;
+
+ complementary_yaw -= data_yaw_to_180;
+#endif
+ if (complementary_yaw > 360) complementary_yaw -= 360;
+ if (complementary_yaw < 0) complementary_yaw += 360;
+
+ yawAngle = complementary_yaw;
+
+}
+
+
+//////////////// SAS ///////////////////
+float integral_rate_roll,integral_rate_pitch,integral_rate_yaw;
+int32_t error_rate_roll = 0, error_rate_pitch = 0, error_rate_yaw = 0;
+int32_t derivate_lpf_roll, derivate_lpf_pitch, derivate_lpf_yaw;
+void SAS_Algorithm(){
+ int16_t stick_roll, stick_pitch, stick_throttle, stick_yaw;
+ stick_roll = GUI.cap[0] * (GUI.limit_rollx100 / 100); // (-100 ~ 100) to (limit angle x 100)
+ stick_pitch = GUI.cap[1] * (GUI.limit_pitchx100 / 100); // (-100 ~ 100) to (limit angle x 100)
+ stick_throttle = (GUI.cap[2] + 100) * 5; // (-100 ~ 100) to (0 ~ 1000)
+ stick_yaw = GUI.cap[3] * 45; // (-100 ~ 100) to (-4500 ~ 4500)
+
+ // WANT YAW - generate //
+ if( ( stick_yaw < -100 ) | ( stick_yaw > 100 ) ){
+ want_yaw += (float)stick_yaw / 10000.0;
+ if(want_yaw < 0) want_yaw += 360;
+ if(want_yaw > 360) want_yaw -= 360;
+ }
+ int16_t error_roll,error_pitch,error_yaw;
+ error_roll = stick_roll - rollAngle*100;
+ error_pitch = stick_pitch - pitchAngle*100;
+
+#ifdef yaw_rate_control
+ error_yaw = stick_yaw;
+#else
+ if(want_yaw <= 180){
+ if( yawAngle <= ( 180 + want_yaw ) ) error_yaw = (want_yaw - yawAngle)*100;
+ else error_yaw = ((want_yaw + 360) - yawAngle )*100;
+ }else{
+ if( yawAngle >= ( want_yaw - 180 ) ) error_yaw = (want_yaw - yawAngle)*100;
+ else error_yaw = (want_yaw - (yawAngle + 360))*100;
+ }
+#endif
+ if(error_roll >= 4500) error_roll = 4500;
+ else if(error_roll <= -4500) error_roll = -4500;
+ if(error_pitch >= 4500) error_pitch = 4500;
+ else if(error_pitch <= -4500) error_pitch = -4500;
+ if(error_yaw >= 3000) error_yaw = 3000;
+ else if(error_yaw <= -3000) error_yaw = -3000;
+
+ /// Convert [-4500 ~ 4500] -->> [-20250 ~ 202500 degx100/s]
+ int16_t earth_want_rate_roll = 0, earth_want_rate_pitch = 0, earth_want_rate_yaw = 0;
+ earth_want_rate_roll = (int32_t)error_roll * GUI.gain_px100[0] / 100.0; // about -100 ~ 100
+ earth_want_rate_pitch = (int32_t)error_pitch * GUI.gain_px100[2] / 100.0;
+ earth_want_rate_yaw = (int32_t)error_yaw * GUI.gain_px100[4] / 100.0;
+
+ int16_t body_want_rate_roll = 0, body_want_rate_pitch = 0, body_want_rate_yaw = 0;
+ float sin_roll, sin_pitch, cos_roll, cos_pitch;
+ sin_roll = sin((rollAngle)*PI / 180);
+ sin_pitch = sin((pitchAngle)*PI / 180);
+ cos_roll = cos((rollAngle)*PI / 180);
+ cos_pitch = cos((pitchAngle)*PI / 180);
+ body_want_rate_roll = earth_want_rate_roll - sin_pitch * earth_want_rate_yaw;
+ body_want_rate_pitch = cos_roll * earth_want_rate_pitch + sin_roll * cos_pitch * earth_want_rate_yaw;
+ body_want_rate_yaw = -sin_roll * earth_want_rate_pitch + cos_pitch * cos_roll * earth_want_rate_yaw;
+
+ int32_t ex_error_rate_roll = 0, ex_error_rate_pitch = 0, ex_error_rate_yaw = 0;
+ ex_error_rate_roll = error_rate_roll;
+ ex_error_rate_pitch = error_rate_pitch;
+ ex_error_rate_yaw = error_rate_yaw;
+ error_rate_roll = ((int32_t)body_want_rate_roll - roll_rate * 100);
+ error_rate_pitch = ((int32_t)body_want_rate_pitch - pitch_rate * 100);
+ error_rate_yaw = ((int32_t)body_want_rate_yaw - yaw_rate * 100);
+// error_rate_roll = ((int32_t)earth_want_rate_roll - roll_rate * 100);
+// error_rate_pitch = ((int32_t)earth_want_rate_pitch - pitch_rate * 100);
+// error_rate_yaw = ((int32_t)earth_want_rate_yaw - yaw_rate * 100);
+
+ //// RATE PID ////
+ //// ---- P Gain ---- ////
+ int16_t stack_roll = 0, stack_pitch = 0, stack_yaw = 0;
+ stack_roll = ((float)error_rate_roll * (float)GUI.gain_px100[1] / 100.0);
+ stack_pitch = ((float)error_rate_pitch * (float)GUI.gain_px100[3] / 100.0);
+ stack_yaw = ((float)error_rate_yaw * (float)GUI.gain_px100[5] / 100.0);
+
+ //// ---- I Gain ---- ////
+ integral_rate_roll += ((float)error_rate_roll * ((float)gap_time/1000000.0) * (float)GUI.gain_ix100[1] / 100.0);
+ integral_rate_pitch += ((float)error_rate_pitch * ((float)gap_time/1000000.0) * (float)GUI.gain_ix100[3] / 100.0);
+ integral_rate_yaw += ((float)error_rate_yaw * ((float)gap_time/1000000.0) * (float)GUI.gain_ix100[5] / 100.0);
+ if(integral_rate_roll > limit_integral_rate_roll) integral_rate_roll = limit_integral_rate_roll;
+ else if(integral_rate_roll < -limit_integral_rate_roll) integral_rate_roll = -limit_integral_rate_roll;
+ if(integral_rate_pitch > limit_integral_rate_pitch) integral_rate_pitch = limit_integral_rate_pitch;
+ else if(integral_rate_pitch < -limit_integral_rate_pitch) integral_rate_pitch = -limit_integral_rate_pitch;
+ if(integral_rate_yaw > limit_integral_rate_yaw) integral_rate_yaw = limit_integral_rate_yaw;
+ else if(integral_rate_yaw < -limit_integral_rate_yaw) integral_rate_yaw = -limit_integral_rate_yaw;
+ stack_roll += integral_rate_roll;
+ stack_pitch += integral_rate_pitch;
+ stack_yaw += integral_rate_yaw;
+
+ //// ------ D Gain ----- /////
+ float derivate_roll, derivate_pitch, derivate_yaw;
+ derivate_roll = ((float)(error_rate_roll - ex_error_rate_roll))/((float)gap_time/1000000.0);
+ derivate_pitch = ((float)(error_rate_pitch - ex_error_rate_pitch))/((float)gap_time/1000000.0);
+ derivate_yaw = ((float)(error_rate_yaw - ex_error_rate_yaw))/((float)gap_time/1000000.0);
+ derivate_roll = (float)derivate_roll * ((float)GUI.gain_dx100[1] / 1000.0);
+ derivate_pitch = (float)derivate_pitch * ((float)GUI.gain_dx100[3] / 1000.0);
+ derivate_yaw = (float)derivate_yaw * ((float)GUI.gain_dx100[5] / 1000.0);
+ derivate_lpf_roll = derivate_lpf_roll * 0.9 + derivate_roll * 0.1;
+ derivate_lpf_pitch = derivate_lpf_pitch * 0.9 + derivate_pitch * 0.1;
+ derivate_lpf_yaw = derivate_lpf_yaw * 0.9 + derivate_yaw * 0.1;
+// derivate_lpf_roll = derivate_roll;
+// derivate_lpf_pitch = derivate_pitch;
+// derivate_lpf_yaw = derivate_yaw;
+ stack_roll += derivate_lpf_roll;
+ stack_pitch += derivate_lpf_pitch;
+ stack_yaw += derivate_lpf_yaw;
+
+ /// stack limit /////
+ if(stack_roll > 5000) stack_roll = 5000;
+ else if(stack_roll < -5000) stack_roll = -5000;
+ if(stack_pitch > 5000) stack_pitch = 5000;
+ else if(stack_pitch < -5000) stack_pitch = -5000;
+ if(stack_yaw > 5000) stack_yaw = 5000;
+ else if(stack_yaw < -5000) stack_yaw = -5000;
+
+ int16_t raw_esc_pwm[4];
+ raw_esc_pwm[0] = ((float)stick_throttle * 7.5) + (+ stack_roll + stack_pitch - stack_yaw);
+ raw_esc_pwm[1] = ((float)stick_throttle * 7.5) + (- stack_roll + stack_pitch + stack_yaw);
+ raw_esc_pwm[2] = ((float)stick_throttle * 7.5) + (- stack_roll - stack_pitch - stack_yaw);
+ raw_esc_pwm[3] = ((float)stick_throttle * 7.5) + (+ stack_roll - stack_pitch + stack_yaw);
+
+ for (int i = 0; i < 4; i++) {
+ if (raw_esc_pwm[i] > 10000) raw_esc_pwm[i] = 10000;
+ else if (raw_esc_pwm[i] < 0) raw_esc_pwm[i] = 0;
+ }
+ uint16_t esc_pwm[4];
+ for (int i = 0; i < 4; i++){
+ if((stick_throttle <= 50)|(GUI.cap[4] <= -50)) {esc_pwm[i] = 0; integral_rate_roll = 0; integral_rate_pitch = 0; integral_rate_yaw = 0; want_yaw = complementary_yaw;}
+ else esc_pwm[i] = raw_esc_pwm[i] + ((float)GUI.motor_min[i]*10);
+ }
+
+ float pwm_data[4];
+ if(GUI.DPN_Info == 0){
+ pwm_data[0] = (float)esc_pwm[0]/10000.0 * (pwm_max - pwm_min) + pwm_min; // 0.439 = 0.9ms , 0.927 = 1.9ms
+ pwm_data[1] = (float)esc_pwm[1]/10000.0 * (pwm_max - pwm_min) + pwm_min; // 0.439 = 0.9ms , 0.927 = 1.9ms
+ pwm_data[2] = (float)esc_pwm[2]/10000.0 * (pwm_max - pwm_min) + pwm_min; // 0.439 = 0.9ms , 0.927 = 1.9ms
+ pwm_data[3] = (float)esc_pwm[3]/10000.0 * (pwm_max - pwm_min) + pwm_min; // 0.439 = 0.9ms , 0.927 = 1.9ms
+ }else if(GUI.DPN_Info == 4){
+ pwm_data[0] = (float)GUI.pwm_info[0]/1000.0 * (pwm_max - pwm_min) + pwm_min;
+ pwm_data[1] = (float)GUI.pwm_info[1]/1000.0 * (pwm_max - pwm_min) + pwm_min;
+ pwm_data[2] = (float)GUI.pwm_info[2]/1000.0 * (pwm_max - pwm_min) + pwm_min;
+ pwm_data[3] = (float)GUI.pwm_info[3]/1000.0 * (pwm_max - pwm_min) + pwm_min;
+ }
+ // Convert [ 0 ~ 10000 ] to [ 0 ~ 200 ] //
+ for (int i = 0; i < 4; i++){
+ if(pwm_data[i] > pwm_max) pwm_data[i] = pwm_max;
+ else if(pwm_data[i] < pwm_min) pwm_data[i] = pwm_min;
+ GUI.pwm[i] = (esc_pwm[i] / 50);
+ }
+ PWM1 = pwm_data[0];
+ PWM2 = pwm_data[1];
+ PWM3 = pwm_data[2];
+ PWM4 = pwm_data[3];
+}
+
+
+float calibrated_cap[6];
+void rc_cap_calibrate(){
+ float tmp_rcv_lpf_cap[6];
+ tmp_rcv_lpf_cap[0] = (RCV1.lpf_cap[0]-150)*2;
+ tmp_rcv_lpf_cap[1] = (RCV2.lpf_cap[1]-150)*2;
+ tmp_rcv_lpf_cap[2] = (RCV3.lpf_cap[2]-150)*2;
+ tmp_rcv_lpf_cap[3] = (RCV4.lpf_cap[3]-150)*2;
+ tmp_rcv_lpf_cap[4] = (RCV5.lpf_cap[4]-150)*2;
+ tmp_rcv_lpf_cap[5] = (RCV6.lpf_cap[5]-150)*2;
+ for(int i=0; i<6; i++){
+ if( tmp_rcv_lpf_cap[i] >= GUI.cap_neu[i] ) {
+ calibrated_cap[i] = (float)(tmp_rcv_lpf_cap[i] - GUI.cap_neu[i]) * 100 / (GUI.cap_max[i] - GUI.cap_neu[i]);
+ }
+ else{
+ calibrated_cap[i] = (float)(tmp_rcv_lpf_cap[i] - GUI.cap_neu[i]) * 100 / (GUI.cap_neu[i] - GUI.cap_min[i]);
+ }
+ ///// MIN MAX ////
+ if (calibrated_cap[i] >= 100) calibrated_cap[i] = 100;
+ else if (calibrated_cap[i] <= -100) calibrated_cap[i] = -100;
+ }
+ ////// SELECT Between EXT_UART and RC Receiver /////
+ GUI.cap[4] = calibrated_cap[4];
+ GUI.cap[5] = calibrated_cap[5];
+ for(int i=0; i<4; i++){
+ if(GUI.cap[4] > 50){
+ GUI.cap[i] = EXT_UART.uart_stick_cap[i];
+ }else{
+ GUI.cap[i] = calibrated_cap[i];
+ }
+ }
+}
+
+
+
+int cnt_1hz;
+int current_system_time;
+int ex_millis_2ms,ex_millis_5ms,ex_millis_20ms,ex_millis_50ms,ex_millis_100ms,ex_millis_1000ms;
+int ex_millis_ahrs_time;
+int cnt;
+uint8_t ex_Compass_Calibration_Mode;
+int main() {
+ /// PWM ////
+ PWM1 = 0;
+ PWM2 = 0;
+ PWM3 = 0;
+ PWM4 = 0;
+ PWM1.period(0.00205); // set PWM period to 2.05ms
+ PWM2.period(0.00205); // set PWM period to 2.05ms
+ PWM3.period(0.00205); // set PWM period to 2.05ms
+ PWM4.period(0.00205); // set PWM period to 2.05ms
+ PWM1 = pwm_min; // set duty cycle to 0.9ms
+ PWM2 = pwm_min; // set duty cycle to 0.9ms
+ PWM3 = pwm_min; // set duty cycle to 0.9ms
+ PWM4 = pwm_min; // set duty cycle to 0.9ms
+
+ MainTimer.start();
+ GUI.Init();
+ LED.Init();
+ RCV1.Init();
+// RCV2.Init();
+// RCV3.Init();
+// RCV4.Init();
+// RCV5.Init();
+// RCV6.Init();
+ ahrs_init();
+ wait(1);
+ //// Data from Data Modem ////
+// rc_cap_neu[0] = 1020; rc_cap_neu[1] = 1027; rc_cap_neu[2] = 1024; rc_cap_neu[3] = 1024; rc_cap_neu[4] = 1024; rc_cap_neu[5] = 1024; rc_cap_neu[6] = 1024; rc_cap_neu[7] = 1024;
+// rc_cap_min[0] = 352; rc_cap_min[1] = 352; rc_cap_min[2] = 352; rc_cap_min[3] = 352; rc_cap_min[4] = 352; rc_cap_min[5] = 352; rc_cap_min[6] = 352; rc_cap_min[7] = 352;
+// rc_cap_max[0] = 1696; rc_cap_max[1] = 1696; rc_cap_max[2] = 1696; rc_cap_max[3] = 1696; rc_cap_max[4] = 1696; rc_cap_max[5] = 1696; rc_cap_max[6] = 1696; rc_cap_max[7] = 1696;
+// limit_rollx100 = 4500,limit_pitchx100 = 4500;
+// gain_px100[0] = 450; gain_px100[1] = 15; gain_ix100[1] = 10; gain_dx100[1] = 4;
+// gain_px100[2] = 450; gain_px100[3] = 15; gain_ix100[3] = 10; gain_dx100[3] = 4;
+// gain_px100[4] = 450; gain_px100[5] = 60; gain_ix100[5] = 20; gain_dx100[5] = 0;
+ limit_integral_rate_roll = 500; limit_integral_rate_pitch = 500; limit_integral_rate_yaw = 500;
+// init_motor_gap[0] = 49;
+// init_motor_gap[1] = 49;
+// init_motor_gap[2] = 49;
+// init_motor_gap[3] = 49;
+
+ led2 = 1;
+ led3 = 0;
+ while(1){
+ wait(0.5);
+ ahrs_init();
+ wait(0.005);
+ compFilter();
+
+ float gyro_limit = 0.3;
+ if( ( gyroData[0]*gRes >= -gyro_limit ) & ( gyroData[0]*gRes <= gyro_limit ) & (gyroData[1]*gRes >= -gyro_limit ) & ( gyroData[1]*gRes <= gyro_limit ) & ( gyroData[2]*gRes >= -gyro_limit ) & ( gyroData[2]*gRes <= gyro_limit ) ){break;}
+ }
+ led2 = 0;
+ led3 = 1;
+ ahrs_lpf_alpha = 0;
+ current_system_time = MainTimer.read_us();
+ ex_millis_ahrs_time = current_system_time - 500;
+ compFilter();
+
+ led2 = 0;
+ led3 = 0;
+
+ ahrs_lpf_alpha = 0.997;
+// while( ( RCV1.rc_state[0] & RCV2.rc_state[1] & RCV3.rc_state[2] & RCV4.rc_state[3] & RCV5.rc_state[4] & RCV6.rc_state[5] ) == false ){
+ while( ( RCV1.rc_state[0] & RCV2.rc_state[1] & RCV3.rc_state[2] & RCV4.rc_state[3] & RCV5.rc_state[4] ) == false ){
+ led3 = 1; wait(0.5);
+ led3 = 0; wait(0.5);
+ }
+
+ compass_init();
+
+ LED.HeadlightPeriod = GUI.headlight_period;
+ LED.HeadlightDutyrate = GUI.headlight_dutyrate;
+ LED.SidelightPeriod = GUI.sidelight_period;
+ LED.SidelightDutyrate = GUI.sidelight_dutyrate;
+
+ EXT_UART.Init();
+ while(1) {
+ GUI.pc_rx_update();
+ LED.Update();
+ RCV1.Update();
+ RCV2.Update();
+ RCV3.Update();
+ RCV4.Update();
+ RCV5.Update();
+ RCV6.Update();
+ EXT_UART.Receive();
+ EXT_UART.Trans();
+ current_system_time = MainTimer.read_us();
+ /// 500 Hz ///
+ if(current_system_time - ex_millis_2ms >= 2000){
+ ex_millis_2ms = current_system_time;
+ }
+ /// 200 Hz ///
+ if(current_system_time - ex_millis_5ms >= 5000){
+ ex_millis_5ms = current_system_time;
+ compFilter();
+ ex_millis_ahrs_time = current_system_time;
+ SAS_Algorithm();
+ }
+ /// 50 Hz ///
+ if(current_system_time - ex_millis_20ms >= 20000){
+ ex_millis_20ms = current_system_time;
+ rc_cap_calibrate();
+ }
+ /// 20 Hz ///
+ if(current_system_time - ex_millis_50ms >= 50000){
+ ex_millis_50ms = current_system_time;
+ /////// EXT UART TRANS ///////////
+ EXT_UART.roll = rollAngle;
+ EXT_UART.pitch = pitchAngle;
+ EXT_UART.yaw = yawAngle;
+ EXT_UART.want_yaw = want_yaw;
+ for(int i=0; i<8; i++){
+ EXT_UART.cap[i] = GUI.cap[i];
+ }
+ for(int i=0; i<8; i++){
+ EXT_UART.pwm[i] = GUI.pwm[i];
+ }
+ for(int i=0; i<8; i++){
+ EXT_UART.debug[i] = 0;
+ }
+ EXT_UART.Refresh();
+ }
+ /// 10 Hz ///
+ if(current_system_time - ex_millis_100ms >= 100000){
+ ex_millis_100ms = current_system_time;
+ }
+ /// 1 Hz ///
+ if(current_system_time - ex_millis_1000ms >= 1000000){
+ ex_millis_1000ms = current_system_time;
+// printf("x: %f \t\ty: %f \t\t z: %f \t\t heading: %f \t\r\n", x, y, z, yawAngle);
+// printf("MAG X : %f \t\tMAG Y: %f \t\t MAG Z: %f \t\t heading: %f \t\r\n", mag_x_avg, mag_y_avg, mag_z_avg, yawAngle);
+// printf("RAW x: %f \t\tRAW y: %f \t\t RAW z: %f \t\t heading: %f \t\r\n", raw_mag_x, raw_mag_y, raw_mag_z, heading);
+// printf("MAX x: %f \t\tMAX y: %f \t\t heading: %f \t\r\n", max_m_x, max_m_y, heading);
+// printf("MIN x: %f \t\tMIN y: %f \t\t heading: %f \t\r\n", min_m_x, min_m_y, heading);
+// printf("avg x: %f \t\tavg y: %f \t\t avg z: %f \t\t heading: %f \t\r\n", avg_m_x, avg_m_y, avg_m_z, heading);
+// printf("Roll: %f \t\tPitch: %f \t\t Yaw: %f\t\r\n", rollAngle, pitchAngle, yawAngle);
+ }
+
+
+
+
+ /////////////////////// GUI //////////////////////////
+ if(GUI.rx_bool() == true){
+ cnt ++;
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //////////////////////////////////////////// [M/S] Mode and State ///////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ GUI.Mode1 = 8 * sin( PI/180 + cnt/50.0) + 8;
+ GUI.Mode2 = 8 * cos( PI/180 + cnt/50.0) + 8;
+ GUI.MissionState = 8 * sin( PI/180 + cnt/50.0) + 8;
+ GUI.CurrentMarker = 11 * cos( PI/180 + cnt/100.0) + 10;
+ GUI.Bat1 = 50 * sin(PI/180 + cnt/50.0) + 50; // 21.6 ~ 24.6 ( 6cell )
+ GUI.Bat2 = 50 * cos(PI/180 + cnt/50.0) + 50; // 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 = cnt * 10;
+ GUI.latitude = 35117030 + 10000 * sin( PI/180 + cnt/50.0);
+ GUI.longitude = 129087896 + 10000 * cos( PI/180 + cnt/50.0);
+ GUI.altitude = 22768 + 32768 * sin( PI/180 + cnt/50.0);
+ GUI.SatNum = 10 + 10 * sin( PI/180 + cnt/50.0);
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ ///////////////////////////////////////////////// AHRS //////////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ GUI.rollx100 = (rollAngle * 100);
+ GUI.pitchx100 = (pitchAngle * 100);
+ GUI.yawx100 = (yawAngle * 100);
+ GUI.roll_ratex100 = roll_rate * 100;
+ GUI.pitch_ratex100 = pitch_rate * 100;
+ GUI.yaw_ratex100 = yaw_rate * 100;
+ GUI.VXx100 = 1000 * sin(PI/180 + cnt/50.0);
+ GUI.VYx100 = 1000 * cos(PI/180 + cnt/50.0);
+ GUI.VZx100 = 1000 * sin(PI/180 + cnt/50.0 + PI/4);
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ ////////////////////////////////////////////// [C/P] CAP/PWM ////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /////////////////////////////////////////// [E/D] Extra & Debug /////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ GUI.DEBUGx100[0] = 9000 * sin(PI/180 + cnt/50.0) + 9000;
+ GUI.DEBUGx100[1] += 100;
+ if(GUI.DEBUGx100[1] >= 36000) GUI.DEBUGx100[1] = 0;
+ GUI.DEBUGx100[2] = 25000 * sin(PI/180 + cnt/50.0) + 25000;
+ GUI.DEBUGx100[2] = raw_mag_x * 100 + 180;
+
+
+ GUI.DEBUGx100[4] = ax*100 + 10000;
+ GUI.DEBUGx100[5] = ay*100 + 10000;
+ GUI.DEBUGx100[6] = az*100 + 10000;
+ GUI.DEBUGx100[4] = GUI.cal_accel_bias[0] * 100 + 10000;
+ GUI.DEBUGx100[5] = GUI.cal_accel_bias[1] * 100 + 10000;
+ GUI.DEBUGx100[6] = GUI.cal_accel_bias[2] * 100 + 10000;
+ GUI.DEBUGx100[7] = MainTimer.read_us()/100000;
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /////////////////////////////////////////// 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] = (RCV1.lpf_cap[0] - 150)*2;
+ GUI.raw_cap[1] = (RCV2.lpf_cap[1] - 150)*2;
+ GUI.raw_cap[2] = (RCV3.lpf_cap[2] - 150)*2;
+ GUI.raw_cap[3] = (RCV4.lpf_cap[3] - 150)*2;
+ GUI.raw_cap[4] = (RCV5.lpf_cap[4] - 150)*2;
+ GUI.raw_cap[5] = (RCV6.lpf_cap[5] - 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();
+ }else{
+ //// calibrating ... /////
+ magnetic_calibration();
+ }
+ }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.Refresh();
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////////
+ }
+}
+
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Apr 16 07:16:00 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/c0f6e94411f5 \ No newline at end of file