main.cpp

00001 #include "mbed.h"
00002 #include <math.h>
00003 #include "Function.h"
00004 #include "millis.h"
00005 #include "SPATIAL.h"
00006 #include "ROBOFRIEN_GUI.h"
00007 #include "ROBOFRIEN_LED.h"
00008 #include "ROBOFRIEN_MOTOR.h"
00009 #include "ROBOFRIEN_PID.h"
00010 #include "ROBOFRIEN_SBUS.h"
00011 #include "ROBOFRIEN_BAT.h"
00012 
00013 
00014 
00015 /* ************  USER INTERFACE ************** */
00016 #define SENSOR_WAIT                 1
00017 #define THROTTLE_MAX                7000
00018 #define THROTTLE_BASE               4000
00019 #define THROTTLE_MIN                3000
00020 #define THROTTLE_CONTROL_LIMIT      1500
00021 #define LIMIT_PROGRESS_VELOCITY     10
00022 #define LIMIT_UP_VELOCITY           4
00023 #define LIMIT_DOWN_VELOCITY         2
00024 #define LIMIT_PLANE_VEL_INTEG       5           // Degree
00025 #define LIMIT_VERTICAL_VEL_INTEG    500
00026 /* ******************************************* */
00027 
00028 /* *************************** */
00029 // AutoFlight Sequence
00030 // 0. Wait to Start
00031 // 1. Take Off  (10m From Ground)
00032 // 2. Heading Align to Marker
00033 // 3. Go to Point ( Pitching & Altitude )
00034 // 4. Hovering
00035 // 5. Landing
00036 #define WAIT_TO_START   1
00037 #define TAKE_OFF        2
00038 #define HEADING_ALIGN   3
00039 #define GO_TO_POINT     4
00040 #define CHECK_MARKER    5
00041 #define LANDING         6
00042 #define WAIT            7
00043 int ex_AUTOFLIGHT_SEQUENCE;
00044 int AUTOFLIGHT_SEQUENCE = WAIT_TO_START;
00045 unsigned long AUTOFLIGHT_TIME_CNT;
00046 /* *************************** */
00047 
00048 ROBOFRIEN_GUI   GUI;
00049 ROBOFRIEN_LED   LED;
00050 ROBOFRIEN_MOTOR MOTOR;
00051 ROBOFRIEN_PID   PID;
00052 SPATIAL         SPATIAL;
00053 ROBOFRIEN_SBUS  SBUS;
00054 ROBOFRIEN_BAT   BAT;
00055 
00056 
00057 void GUI_UPDATE();
00058 void ALGORITHM();
00059 void ANGLE_CONTROL(double [3]);
00060 void PLANE_VEL_CONTROL(double, double, double *, double *);
00061 void VERTICAL_VEL_CONTROL(double, double *);
00062 void VEL_INTEG_RESET();
00063 float DEG_TO_METER(float , float , float , float );
00064 
00065 unsigned long millis_100hz, millis_10hz, millis_200hz;
00066 double want_Angle[3];
00067 double want_Rate[3];
00068 double err_Angle[3];
00069 double THROTTLE_VALUE;
00070 double want_LNG, want_LAT, want_ALT;
00071 int main()
00072 {
00073   LED.Init();
00074   MOTOR.Init();
00075   wait(1);
00076   SPATIAL.Init(SENSOR_WAIT);
00077   SBUS.Init();
00078   PID.Init();
00079   GUI.Init();
00080   BAT.Init(3);        // 3cell
00081   millisStart();
00082   while (1) {
00083     GUI.pc_rx_update();
00084     GUI.TRANS_BUFFER();
00085     LED.update(GUI.headlight_period, GUI.headlight_dutyrate, GUI.sidelight_period, GUI.sidelight_dutyrate );
00086     SPATIAL.SPATIAL_RECEIVE();
00087     SBUS.update();
00088     if ( ( millis() - millis_200hz ) > 5 ) {
00089       millis_200hz = millis();
00090       if ( SPATIAL.SPATIAL_STABLE() == false ) {
00091         LED.ALARM(1);
00092         if (GUI.button[0] == true) {
00093           ALGORITHM();
00094           MOTOR.update();
00095         } else {
00096           MOTOR.OFF();
00097         }
00098       } else {
00099         LED.ALARM(0);
00100         ALGORITHM();
00101         MOTOR.update();
00102       }
00103     }
00104 
00105     if ( (millis() - millis_10hz ) > 100) {
00106       millis_10hz = millis();
00107       GUI_UPDATE();
00108       GUI.Trans();
00109       GUI.SET_DATA();
00110     }
00111     if (GUI.rx_bool() == true) {
00112       GUI.Refresh();
00113     }
00114   }
00115 }
00116 
00117 
00118 uint8_t EX_CONTROLLER_STATE;
00119 bool MOTOR_FORCED_OFF_BOOL = true;
00120 void ALGORITHM() {       // 200 Hz //
00121   /* *************************** */
00122   /* ******* Want Angle ******** */
00123   /* *************************** */
00124   switch (SBUS.CONTROLLER_STATE) {
00125     case SIGNAL_OFF: {
00126         want_Angle[0] = 0;  want_Angle[1] = 0;  want_Angle[2] = SPATIAL.YAW;
00127         THROTTLE_VALUE = 0;
00128       } break;
00129     case MOTOR_OFF: {
00130         MOTOR_FORCED_OFF_BOOL = true;
00131         want_Angle[0] = 0;  want_Angle[1] = 0;  want_Angle[2] = SPATIAL.YAW;
00132         THROTTLE_VALUE = 0;
00133       } break;
00134     case MANUAL_ATTITUDE: {
00135         MOTOR_FORCED_OFF_BOOL = false;
00136         want_Angle[0] = ((SBUS.channel[0]) / 1000.0) * GUI.limit_rollx100 / 100.0;       // Want Angle : Roll
00137         want_Angle[1] = ((SBUS.channel[1]) / 1000.0) * GUI.limit_pitchx100 / 100.0;      // Want Angle : Pitch
00138         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);
00139         if ( want_Angle[2] > 360) want_Angle[2] -= 360;
00140         else if ( want_Angle[2] < 0) want_Angle[2] += 360;
00141         THROTTLE_VALUE = ((SBUS.channel[2] + 1000) / 2000.0 * THROTTLE_MAX);
00142       } break;
00143     case MANUAL_VELOCITY ... AUTO_FLIGHT: {         // MANUAL_VELOCITY(3), MANUAL_POSITION(4), AUTO_FLIGHT(5) //
00144         //// [LNG]Longitude Up : East Face, Longitude Down : West Face
00145         //// [LAT]Latitude Up : North Face, Latitude Down : South Face
00146         if ( EX_CONTROLLER_STATE != SBUS.CONTROLLER_STATE) {
00147           want_LAT = SPATIAL.LATITUDE;
00148           want_LNG = SPATIAL.LONGITUDE;
00149           want_ALT = SPATIAL.HEIGHT;
00150           AUTOFLIGHT_TIME_CNT = 0;
00151         }
00152 
00153         double want_Vel[3];  // VEL[0] = X-axis, VEL[1] = Y-axis, VEL[2] = Z-axis
00154         if ( SBUS.CONTROLLER_STATE == MANUAL_VELOCITY ) {
00155           /* ******************************************************* */
00156           /* ****************** MANUAL VELOCITY ******************** */
00157           /* ******************************************************* */
00158           MOTOR_FORCED_OFF_BOOL = false;
00159           want_Vel[0] = -(SBUS.channel[1] / 1000.0) * LIMIT_PROGRESS_VELOCITY;
00160           want_Vel[1] = (SBUS.channel[0] / 1000.0) * LIMIT_PROGRESS_VELOCITY;
00161           if ( SBUS.channel[2] >= 0 )      want_Vel[2] = (SBUS.channel[2] / 1000.0) * LIMIT_UP_VELOCITY;
00162           else if ( SBUS.channel[2] < 0 )  want_Vel[2] = (SBUS.channel[2] / 1000.0) * LIMIT_DOWN_VELOCITY;
00163           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);
00164 
00165         } else {
00166           if ( SBUS.CONTROLLER_STATE == MANUAL_POSITION ) {
00167             /* ******************************************************* */
00168             /* ****************** MANUAL POSITION ******************** */
00169             /* ******************************************************* */
00170             MOTOR_FORCED_OFF_BOOL = false;
00171             double del_pos[3];
00172             del_pos[0] = (-(SBUS.channel[1] / 1000.0) * LIMIT_PROGRESS_VELOCITY);  //divided by time
00173             del_pos[1] = ((SBUS.channel[0] / 1000.0) * LIMIT_PROGRESS_VELOCITY);   //divided by time
00174             if (SBUS.channel[2] >= 0)        del_pos[2] = ((SBUS.channel[2] / 1000.0) * LIMIT_UP_VELOCITY);  //divided by time
00175             else if (SBUS.channel[2] < 0)    del_pos[2] = ((SBUS.channel[2] / 1000.0) * LIMIT_DOWN_VELOCITY);  //divided by time
00176 
00177             double NED_del_pos[3];
00178             NED_del_pos[0] = del_pos[0] * cos(SPATIAL.YAW * M_PI / 180.0) - del_pos[1] * sin(SPATIAL.YAW * M_PI / 180.0);
00179             NED_del_pos[1] = del_pos[0] * sin(SPATIAL.YAW * M_PI / 180.0) + del_pos[1] * cos(SPATIAL.YAW * M_PI / 180.0);
00180 
00181             if( (SBUS.channel[1] < 5) & (SBUS.channel[1] > -5) ) {}
00182             else want_LAT += (NED_del_pos[0] / 200.0) / 110574.0;
00183             if( (SBUS.channel[0] < 5) & (SBUS.channel[0] > -5) ) {}
00184             else want_LNG += (NED_del_pos[1] / 200.0) / 111320.0;
00185             if( (SBUS.channel[2] < 10) & (SBUS.channel[2] > -10) ) {}
00186             else want_ALT += (del_pos[2] / 200.0);
00187 
00188             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);
00189 
00190           } else if ( SBUS.CONTROLLER_STATE == AUTO_FLIGHT) {
00191             /* ******************************************************* */
00192             /* ******************    AUTO FLIGHT  ******************** */
00193             /* ******************************************************* */
00194             if (AUTOFLIGHT_SEQUENCE != ex_AUTOFLIGHT_SEQUENCE) {
00195               AUTOFLIGHT_TIME_CNT = 0;
00196             }
00197             ex_AUTOFLIGHT_SEQUENCE = AUTOFLIGHT_SEQUENCE;
00198             AUTOFLIGHT_TIME_CNT ++;
00199             if ( AUTOFLIGHT_SEQUENCE == WAIT_TO_START) MOTOR_FORCED_OFF_BOOL = true;
00200             else MOTOR_FORCED_OFF_BOOL = false;
00201             switch (AUTOFLIGHT_SEQUENCE) {
00202               case WAIT_TO_START: {
00203                   if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) AUTOFLIGHT_SEQUENCE = TAKE_OFF;
00204                   want_LAT = SPATIAL.LATITUDE;
00205                   want_LNG = SPATIAL.LONGITUDE;
00206                   want_ALT = SPATIAL.HEIGHT;
00207                   want_Angle[2] = SPATIAL.YAW;
00208                   break;
00209                 }
00210               case TAKE_OFF: {            /// Take off [ Ground --> 5m ]  in 1m/s
00211                   want_ALT += (1 / 200.0);
00212                   if ( AUTOFLIGHT_TIME_CNT > (5 * 200)) {
00213                     AUTOFLIGHT_SEQUENCE = CHECK_MARKER;
00214                     GUI.CurrentMarker = 1;
00215                   }
00216                   break;
00217                 }
00218               case HEADING_ALIGN: {
00219                   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);
00220                   want_Angle[2] = BEARING_ANGLE;
00221                   if ( ABSOL((BEARING_ANGLE - SPATIAL.YAW)) < 5 ) {
00222                     if (AUTOFLIGHT_TIME_CNT > (3 * 200) ) AUTOFLIGHT_SEQUENCE = GO_TO_POINT;
00223                   } else AUTOFLIGHT_TIME_CNT = 0;
00224 //                  GUI.DEBUGx100[0] = (BEARING_ANGLE + 100) * 100;
00225 //                  GUI.DEBUGx100[1] = (ABSOL((BEARING_ANGLE - SPATIAL.YAW)) + 100) * 100;
00226                   break;
00227                 }
00228               case GO_TO_POINT: {
00229                   float Target_LAT = GUI.Marker_Lat[GUI.CurrentMarker - 1] / 1000000.0;
00230                   float Target_LNG = GUI.Marker_Lng[GUI.CurrentMarker - 1] / 1000000.0;
00231                   float Target_ALT = GUI.Marker_Alt[GUI.CurrentMarker - 1] / 100.0;
00232 //                  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);
00233 //                  want_Angle[2] = BEARING_ANGLE;
00234                   // 1ms ++
00235                   /*                            if(Target_LAT > want_LAT) want_LAT += (1.5/200.0)/110574.0;
00236                                               else want_LAT -= (1/200.0)/110574.0;
00237                                               if(Target_LNG > want_LNG) want_LNG += (1.5/200.0)/111320.0;
00238                                               else want_LNG -= (1/200.0)/111320.0;
00239                                               if(Target_ALT > want_ALT) want_ALT += (1/200.0);
00240                                               else want_ALT -= (1/200.0);
00241                   */
00242 //                  want_LAT = Target_LAT;
00243 //                  want_LNG = Target_LNG;
00244 //                  want_ALT = Target_ALT;
00245                   if(Target_LAT > want_LAT) want_LAT += (LIMIT_PROGRESS_VELOCITY/200.0)/110574.0;
00246                   else want_LAT -= (LIMIT_PROGRESS_VELOCITY/200.0)/110574.0;
00247                   if(Target_LNG > want_LNG) want_LNG += (LIMIT_PROGRESS_VELOCITY/200.0)/111320.0;
00248                   else want_LNG -= (LIMIT_PROGRESS_VELOCITY/200.0)/111320.0;
00249                   if(Target_ALT > want_ALT) want_ALT += (LIMIT_UP_VELOCITY/200.0);
00250                   else want_ALT -= (LIMIT_DOWN_VELOCITY/200.0);
00251 
00252                   bool escape_bool = false;
00253                   float POSITION_DISTANCE;
00254                   float HEIGHT_DISTANCE;
00255                   POSITION_DISTANCE = DEG_TO_METER( want_LAT, want_LNG, SPATIAL.LATITUDE, SPATIAL.LONGITUDE );
00256                   HEIGHT_DISTANCE = sqrt( (want_ALT - SPATIAL.HEIGHT) * (want_ALT - SPATIAL.HEIGHT) );
00257                   if ( ( POSITION_DISTANCE < 5 ) & (HEIGHT_DISTANCE < 5)) {     // 5m
00258                     if ( (POSITION_DISTANCE < 3) ) {                            // 3m
00259                       if ( (POSITION_DISTANCE < 1) ) {                          // 1m
00260                         ///////////// Target in the 1m OK !!!! /////////////////
00261                         escape_bool = true;
00262                         /////////////////////////////////////////////////
00263                       }
00264                       else {
00265                         if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) escape_bool = true;       // Time Out //
00266                       }
00267                     }
00268                     else {
00269                       if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) escape_bool = true;           // Time Out //
00270                     }
00271                   }
00272                   else {
00273                     AUTOFLIGHT_TIME_CNT = 0;
00274                   }
00275                   if ( escape_bool == true) {
00276                     AUTOFLIGHT_SEQUENCE = CHECK_MARKER;
00277                     GUI.CurrentMarker ++;
00278                   }
00279                   // 1m내외
00280                   break;
00281                 }
00282               case CHECK_MARKER: {
00283                   if (AUTOFLIGHT_TIME_CNT > (3 * 200) ) {
00284                     if ( (GUI.Marker_Lat[GUI.CurrentMarker - 1] != 0) & (GUI.Marker_Lng[GUI.CurrentMarker - 1] != 0) ) {
00285                       AUTOFLIGHT_SEQUENCE = HEADING_ALIGN;
00286                     } else {
00287                       AUTOFLIGHT_SEQUENCE = LANDING;
00288                     }
00289                   }
00290                   break;
00291                 }
00292               case LANDING: {
00293                   want_ALT -= (0.5 / 200.0);    // 0.5m/s
00294                   break;
00295                 }
00296               case WAIT: {
00297                   break;
00298                 }
00299             }
00300           }
00301           double NED_want_vel[3];
00302           NED_want_vel[0] = (want_LAT - SPATIAL.LATITUDE) * 110574.0 * (GUI.gain_px100[4] / 100.0);
00303           NED_want_vel[1] = (want_LNG - SPATIAL.LONGITUDE) * 111320.0 * (GUI.gain_px100[5] / 100.0);
00304           NED_want_vel[2] = (want_ALT - SPATIAL.HEIGHT) * (GUI.gain_px100[6] / 100.0);
00305 
00306           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);
00307           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);
00308           want_Vel[2] = NED_want_vel[2];
00309 
00310           float LIMIT_PROGRESS_VELOCITY_CALCULATED;
00311           float POSITION_DISTANCE = DEG_TO_METER( want_LAT, want_LNG, SPATIAL.LATITUDE, SPATIAL.LONGITUDE );
00312           if ( POSITION_DISTANCE < 20) LIMIT_PROGRESS_VELOCITY_CALCULATED = (0.04 * POSITION_DISTANCE + 0.2) * LIMIT_PROGRESS_VELOCITY;
00313           else LIMIT_PROGRESS_VELOCITY_CALCULATED = LIMIT_PROGRESS_VELOCITY;
00314 
00315           want_Vel[0] = MIN_MAX(want_Vel[0], -LIMIT_PROGRESS_VELOCITY_CALCULATED, LIMIT_PROGRESS_VELOCITY_CALCULATED);
00316           want_Vel[1] = MIN_MAX(want_Vel[1], -LIMIT_PROGRESS_VELOCITY_CALCULATED, LIMIT_PROGRESS_VELOCITY_CALCULATED);
00317           if (want_Vel[2] >= 0)        want_Vel[2] = MIN_MAX(want_Vel[2], -LIMIT_UP_VELOCITY, LIMIT_UP_VELOCITY);
00318           else if (want_Vel[2] < 0)    want_Vel[2] = MIN_MAX(want_Vel[2], -LIMIT_DOWN_VELOCITY, LIMIT_DOWN_VELOCITY);
00319 
00320         }
00321 
00322         /* ********** WANT ANGLE GENERATE ************ */
00323         PLANE_VEL_CONTROL(want_Vel[0], want_Vel[1], &want_Angle[0], &want_Angle[1]);
00324         if ( want_Angle[2] > 360) want_Angle[2] -= 360;
00325         else if ( want_Angle[2] < 0) want_Angle[2] += 360;
00326 
00327         /* ********** THROTTLE VALUE GENRATE ************* */
00328         VERTICAL_VEL_CONTROL(want_Vel[2], &THROTTLE_VALUE);
00329         if (MOTOR_FORCED_OFF_BOOL == true) THROTTLE_VALUE = 0;
00330 
00331       } break;
00332   }
00333   EX_CONTROLLER_STATE = SBUS.CONTROLLER_STATE;
00334   /* ************************** */
00335   /* ******* Want Rate ******** */
00336   /* ************************** */
00337   ANGLE_CONTROL( want_Angle );
00338 
00339   if (GUI.DPN_Info == 0) {
00340     if ( SBUS.CONTROLLER_STATE <= MOTOR_OFF) {
00341       PID.RAT_INTEGER_RESET();
00342       VEL_INTEG_RESET();
00343       for (int i = 0; i < 6; i++) MOTOR.Value[i] = 0;
00344     } else {
00345       int MOTOR_MIN_DEADZONE  = 5;
00346       for (int i = 0; i < 5; i++) {
00347         if (GUI.motor_min[i] > 100)GUI.motor_min[i] = 100;
00348       }
00349       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];
00350       MOTOR.Value[1] = (GUI.motor_min[1] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE + PID.output_Data[0]        + 0                          + PID.output_Data[2];
00351       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];
00352       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];
00353       MOTOR.Value[4] = (GUI.motor_min[4] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE - PID.output_Data[0]        + 0                          - PID.output_Data[2];
00354       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];
00355       for (int i = 0; i < 6; i++) {
00356         if ( MOTOR.Value[i] < ((GUI.motor_min[0] - MOTOR_MIN_DEADZONE) * 10 ) ) {
00357           MOTOR.Value[i] = ((GUI.motor_min[0] - MOTOR_MIN_DEADZONE) * 10 );
00358         }
00359       }
00360     }
00361   } else if (GUI.DPN_Info == 4) {
00362     MOTOR.Value[0] = (float)GUI.pwm_info[0] * 10;
00363     MOTOR.Value[1] = (float)GUI.pwm_info[1] * 10;
00364     MOTOR.Value[2] = (float)GUI.pwm_info[2] * 10;
00365     MOTOR.Value[3] = (float)GUI.pwm_info[3] * 10;
00366     MOTOR.Value[4] = (float)GUI.pwm_info[4] * 10;
00367     MOTOR.Value[5] = (float)GUI.pwm_info[5] * 10;
00368   }
00369 }
00370 
00371 
00372 void GUI_UPDATE() {
00373   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00374   //////////////////////////////////////////// [M/S] Mode and State ///////////////////////////////////////
00375   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00376   /* ************ MODE 1 ************* */
00377   GUI.Mode1 = SBUS.CONTROLLER_STATE;
00378   GUI.Alarm = !SPATIAL.SPATIAL_STABLE();
00379 
00380   GUI.MissionState = AUTOFLIGHT_SEQUENCE;
00381   GUI.Bat1 = BAT.update();
00382   // [ Button State & Home Point Chksum & Marker Chksum ] is change automatically in "ROBOFRIEN_GUI.cpp" //
00383   // You Can not change this setting, just use it ///
00384   if (GUI.button[0] == true) {}
00385   else {}
00386   if (GUI.button[1] == true) {
00387     AUTOFLIGHT_SEQUENCE = WAIT_TO_START;
00388   }
00389   else {}
00390   if (GUI.button[2] == true) {}
00391   else {}
00392   if (GUI.button[3] == true) {}
00393   else {}
00394   if (GUI.button[4] == true) {}
00395   else {}
00396 
00397   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00398   //////////////////////////////////////////////// [GPS] GPS //////////////////////////////////////////////
00399   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00400   GUI.utc_time = SPATIAL.UNIX_TIME;
00401   GUI.latitude = (SPATIAL.LATITUDE * 1000000);
00402   GUI.longitude = (SPATIAL.LONGITUDE * 1000000);
00403   GUI.altitude =  SPATIAL.HEIGHT * 100;
00404   GUI.SatNum = SPATIAL.SATELLITE_NUM;
00405   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00406   ///////////////////////////////////////////////// AHRS //////////////////////////////////////////////////
00407   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00408   GUI.rollx100 = (SPATIAL.ROLL * 100);
00409   GUI.pitchx100 = (SPATIAL.PITCH * 100);
00410   GUI.yawx100 = ((SPATIAL.YAW ) * 100);
00411   GUI.roll_ratex100 =  SPATIAL.ROLL_RATE * 100;
00412   GUI.pitch_ratex100 =  SPATIAL.PITCH_RATE * 100;
00413   GUI.yaw_ratex100 =  SPATIAL.YAW_RATE * 100;
00414   GUI.VXx100 = SPATIAL.Vx * 100;
00415   GUI.VYx100 = SPATIAL.Vy * 100;
00416   GUI.VZx100 = SPATIAL.Vz * 100;
00417 
00418   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00419   ////////////////////////////////////////////// [C/P] CAP/PWM ////////////////////////////////////////////
00420   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00421 
00422   GUI.cap[0] = SBUS.channel[0] / 10.0;
00423   GUI.cap[1] = SBUS.channel[1] / 10.0;
00424   GUI.cap[2] = SBUS.channel[2] / 10.0;
00425   GUI.cap[3] = SBUS.channel[3] / 10.0;
00426   GUI.cap[4] = SBUS.channel[4] / 10.0;
00427   GUI.cap[5] = SBUS.channel[5] / 10.0;
00428   GUI.cap[6] = SBUS.channel[6] / 10.0;
00429   GUI.cap[7] = SBUS.channel[7] / 10.0;
00430 
00431   GUI.pwm[0] = MOTOR.Value[0] / 100;         // (0 ~ 10000) --> ( 0 ~ 100 )
00432   GUI.pwm[1] = MOTOR.Value[1] / 100;         // (0 ~ 10000) --> ( 0 ~ 100 )
00433   GUI.pwm[2] = MOTOR.Value[2] / 100;         // (0 ~ 10000) --> ( 0 ~ 100 )
00434   GUI.pwm[3] = MOTOR.Value[3] / 100;         // (0 ~ 10000) --> ( 0 ~ 100 )
00435   GUI.pwm[4] = MOTOR.Value[4] / 100;         // (0 ~ 10000) --> ( 0 ~ 100 )
00436   GUI.pwm[5] = MOTOR.Value[5] / 100;         // (0 ~ 10000) --> ( 0 ~ 100 )
00437 
00438   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00439   /////////////////////////////////////////// [E/D] Extra & Debug /////////////////////////////////////////
00440   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00441 
00442   GUI.DEBUGx100[2] = (want_Angle[2] + 100) * 100;
00443   GUI.DEBUGx100[3] = GUI.Controller_Joystick[3];
00444   GUI.DEBUGx100[4] = GUI.Gimbal_Joystick[0];
00445   GUI.DEBUGx100[5] = GUI.Gimbal_Joystick[1];
00446   float PNU_RPM[2];
00447   PNU_RPM[0] = sin( millis() / 10000.0 ) * 10000;
00448   PNU_RPM[1] = cos( millis() / 10000.0 ) * 10000;
00449   GUI.DEBUGx100[6] = PNU_RPM[0];
00450   GUI.DEBUGx100[7] = PNU_RPM[1];
00451 
00452   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00453   /////////////////////////////////////////// Configuration ///////////////////////////////////////////////
00454   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00455 
00456   ///////////////////////////// -----------------DPN 1-------------------- ////////////////////////////////
00457   // You can set the this value from "Config.h"  ( EEPROM_MODEL_TYPE1, EEPROM_MODEL_TYPE2_UP, EEPROM_MODEL_TYPE2_DOWN ) //
00458 
00459   ///////////////////////////// -----------------DPN 2-------------------- ////////////////////////////////
00460   // You can set the this value from "Config.h"  ( MODEL_INFO, FIRMWARE_INFO ) //
00461 
00462   ///////////////////////////// -----------------DPN 3-------------------- ////////////////////////////////
00463   /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) ///
00464   /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value //
00465 
00466   ///////////////////////////// -----------------DPN 4-------------------- ////////////////////////////////
00467   /// You can use the [pwm_info , motor_min[4]] for calibration of motor pwm value //
00468 
00469   ///////////////////////////// -----------------DPN 5-------------------- ////////////////////////////////
00470   /*
00471       LED.HeadlightPeriod = GUI.headlight_period;
00472       LED.HeadlightDutyrate = GUI.headlight_dutyrate;
00473       LED.SidelightPeriod = GUI.sidelight_period;
00474       LED.SidelightDutyrate = GUI.sidelight_dutyrate;
00475   */
00476   ///////////////////////////// -----------------DPN 6-------------------- ////////////////////////////////
00477   /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) ///
00478   /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value //
00479   GUI.raw_cap[0] = (0 - 150) * 2;
00480   GUI.raw_cap[1] = (0 - 150) * 2;
00481   GUI.raw_cap[2] = (0 - 150) * 2;
00482   GUI.raw_cap[3] = (0 - 150) * 2;
00483   GUI.raw_cap[4] = (0 - 150) * 2;
00484   GUI.raw_cap[5] = (0 - 150) * 2;
00485   GUI.raw_cap[6] = 0;
00486   GUI.raw_cap[7] = 0;
00487 
00488   if (GUI.attitude_configuration_bool == true) {
00489     GUI.attitude_configuration_bool = false;
00490     // You can calibration of attitude (Roll, Pitch) //
00491     //                GUI.attitude_calibrate(accelData[0]*aRes,accelData[1]*aRes,(accelData[2]*aRes-1));
00492   }
00493   if (GUI.Compass_Calibration_Mode == 1) {
00494     // You can calibration of compass (Yaw)//
00495     //                if(ex_Compass_Calibration_Mode == 0){
00496     //                    magnetic_offset_reset();
00497     //                }else{
00498     //                    //// calibrating ... /////
00499     //                    magnetic_calibration();
00500     //                }
00501   } else if (GUI.Compass_Calibration_Mode == 2) {
00502     //                // You can finish the calibration of compass
00503     //                //// write to eeprom ////
00504     //                GUI.Compass_Calibration_Mode = 0;
00505     //                GUI.write_compass_setting_to_eeprom();
00506   }
00507   else if (GUI.Compass_Calibration_Mode == 3) {   // declination angle
00508     SPATIAL.DECLINATION_ANGLE = GUI.declination_angle;
00509   }
00510   //            ex_Compass_Calibration_Mode = GUI.Compass_Calibration_Mode;
00511 
00512   ///////////////////////////// -----------------DPN 7-------------------- ////////////////////////////////
00513   /// You can use the [limit_rollx100, limit_pitchx100, limit_roll_ratex100, limit_pitch_ratex100, limit_yaw_ratex100] for limit the angle and angular velocity //
00514 
00515   ///////////////////////////// -----------------DPN 8-------------------- ////////////////////////////////
00516   /// You can use the [gain_px100[20], gain_dx100[20], gain_ix100[20]] for gain tuning //
00517   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00518   //////////////////////////////////////////////// Refresh ////////////////////////////////////////////////
00519   /////////////////////////////////////////////////////////////////////////////////////////////////////////
00520   //    GUI.trans_flight_data(255, 0);
00521   GUI.pc_rx_update();
00522 }
00523 
00524 void ANGLE_CONTROL(double IN_WANT_ANGLE[3]) {
00525   /* ********* MIN - MAX ************* */
00526   IN_WANT_ANGLE[0] = MIN_MAX(IN_WANT_ANGLE[0], -GUI.limit_rollx100 / 100.0, GUI.limit_rollx100 / 100.0 );
00527   IN_WANT_ANGLE[1] = MIN_MAX(IN_WANT_ANGLE[1], -GUI.limit_pitchx100 / 100.0, GUI.limit_pitchx100 / 100.0 );
00528   err_Angle[0] = IN_WANT_ANGLE[0] - SPATIAL.ROLL;
00529   err_Angle[1] = IN_WANT_ANGLE[1] - SPATIAL.PITCH;
00530   err_Angle[2] = IN_WANT_ANGLE[2] - SPATIAL.YAW;
00531   if (err_Angle[2] > 180) err_Angle[2] -= 360;
00532   else if (err_Angle[2] < -180) err_Angle[2] += 360;
00533   want_Rate[0] = err_Angle[0] * GUI.gain_px100[0] / 100.0;    // Want Rate : Roll Rate
00534   want_Rate[1] = err_Angle[1] * GUI.gain_dx100[0] / 100.0;    // Want Rate : Pitch Rate
00535   want_Rate[2] = err_Angle[2] * GUI.gain_ix100[0] / 100.0;    // Want Rate : Yaw Rate
00536   want_Rate[0] = MIN_MAX(want_Rate[0], -GUI.limit_roll_ratex100 / 100.0, GUI.limit_roll_ratex100 / 100.0);
00537   want_Rate[1] = MIN_MAX(want_Rate[1], -GUI.limit_pitch_ratex100 / 100.0, GUI.limit_pitch_ratex100 / 100.0);
00538   want_Rate[2] = MIN_MAX(want_Rate[2], -GUI.limit_yaw_ratex100 / 100.0, GUI.limit_yaw_ratex100 / 100.0);
00539   PID.err_Rate[0] = (want_Rate[0] - SPATIAL.ROLL_RATE);
00540   PID.err_Rate[1] = (want_Rate[1] - SPATIAL.PITCH_RATE);
00541   PID.err_Rate[2] = (want_Rate[2] - SPATIAL.YAW_RATE);
00542   PID.RAT_KP[0] = -GUI.gain_px100[1] / 100.0;
00543   PID.RAT_KP[1] = -GUI.gain_px100[2] / 100.0;
00544   PID.RAT_KP[2] = -GUI.gain_px100[3] / 100.0;
00545   PID.RAT_KI[0] = -GUI.gain_ix100[1] / 100.0;
00546   PID.RAT_KI[1] = -GUI.gain_ix100[2] / 100.0;
00547   PID.RAT_KI[2] = -GUI.gain_ix100[3] / 100.0;
00548   PID.update();
00549 }
00550 
00551 double PLANE_VEL_INTEG[2];
00552 void PLANE_VEL_CONTROL(double IN_VEL_X, double IN_VEL_Y, double *OUTPUT_WANT_ROLL, double *OUTPUT_WANT_PITCH) {
00553   double err_Vel[2];
00554   err_Vel[0] = ( IN_VEL_X - SPATIAL.Vx );
00555   err_Vel[1] = ( IN_VEL_Y - SPATIAL.Vy );
00556   err_Vel[0] = MIN_MAX(err_Vel[0], -LIMIT_PROGRESS_VELOCITY, LIMIT_PROGRESS_VELOCITY);
00557   err_Vel[1] = MIN_MAX(err_Vel[1], -LIMIT_PROGRESS_VELOCITY, LIMIT_PROGRESS_VELOCITY);
00558   *OUTPUT_WANT_PITCH = -(err_Vel[0] * (GUI.gain_dx100[4] / 100.0));
00559   *OUTPUT_WANT_ROLL  = +(err_Vel[1] * (GUI.gain_dx100[5] / 100.0));
00560 
00561   /* ******** VEL PID - I **************** */
00562   PLANE_VEL_INTEG[0] -= (err_Vel[0]) * (GUI.gain_ix100[4] / 100.0);
00563   PLANE_VEL_INTEG[1] += (err_Vel[1]) * (GUI.gain_ix100[5] / 100.0);
00564   PLANE_VEL_INTEG[0] = MIN_MAX(PLANE_VEL_INTEG[0], -LIMIT_PLANE_VEL_INTEG, LIMIT_PLANE_VEL_INTEG);
00565   PLANE_VEL_INTEG[1] = MIN_MAX(PLANE_VEL_INTEG[1], -LIMIT_PLANE_VEL_INTEG, LIMIT_PLANE_VEL_INTEG);
00566   *OUTPUT_WANT_PITCH += PLANE_VEL_INTEG[0];
00567   *OUTPUT_WANT_ROLL  += PLANE_VEL_INTEG[1];
00568 
00569 }
00570 
00571 double VERTICAL_VEL_INTEG;
00572 void VERTICAL_VEL_CONTROL(double IN_VEL_Z, double *OUTPUT_THROTTLE_VALUE) {
00573   double err_Vel_Z;
00574   err_Vel_Z = IN_VEL_Z - SPATIAL.Vz;
00575   if (err_Vel_Z >= 0) err_Vel_Z = MIN_MAX(err_Vel_Z, -LIMIT_UP_VELOCITY, LIMIT_UP_VELOCITY);
00576   else err_Vel_Z = MIN_MAX(err_Vel_Z, -LIMIT_DOWN_VELOCITY, LIMIT_DOWN_VELOCITY);
00577   *OUTPUT_THROTTLE_VALUE = (err_Vel_Z * 10) * (GUI.gain_dx100[6] / 100.0);
00578 
00579   VERTICAL_VEL_INTEG += (err_Vel_Z) * (GUI.gain_ix100[6] / 100.0);
00580   VERTICAL_VEL_INTEG = MIN_MAX(VERTICAL_VEL_INTEG, -LIMIT_VERTICAL_VEL_INTEG, LIMIT_VERTICAL_VEL_INTEG);
00581   *OUTPUT_THROTTLE_VALUE += VERTICAL_VEL_INTEG;
00582 
00583   *OUTPUT_THROTTLE_VALUE = MIN_MAX(*OUTPUT_THROTTLE_VALUE, -THROTTLE_CONTROL_LIMIT, THROTTLE_CONTROL_LIMIT);
00584 
00585   *OUTPUT_THROTTLE_VALUE += THROTTLE_BASE;
00586 
00587   *OUTPUT_THROTTLE_VALUE = MIN_MAX( *OUTPUT_THROTTLE_VALUE, THROTTLE_MIN, THROTTLE_MAX );
00588 
00589 }
00590 
00591 void VEL_INTEG_RESET() {
00592   PLANE_VEL_INTEG[0] = 0; PLANE_VEL_INTEG[1] = 0; VERTICAL_VEL_INTEG = 0;
00593 }
00594 
00595 float DEG_TO_METER(float lat1, float lon1, float lat2, float lon2) {
00596   float R = 6378.137;     // Radius of earth in KM
00597   float dLat = lat2 * M_PI / 180.0 - lat1 * M_PI / 180.0;
00598   float dLon = lon2 * M_PI / 180.0 - lon1 * M_PI / 180.0;
00599   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);
00600   float c = 2 * atan2(sqrt(a), sqrt(1 - a));
00601   float d = R * c;
00602   return d * 1000;    // meters
00603 }