Kim Youngsik / Mbed 2 deprecated 0ROBOFRIEN_FCC_v1_12

Dependencies:   mbed BufferedSerial ConfigFile

main.cpp@1:9530746906b6, 2018-11-28 (annotated)

Committer:
skyyoungsik
Date:
Wed Nov 28 13:06:23 2018 +0000
Revision:
1:9530746906b6
Parent:
0:3473b92e991e 
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skyyoungsik 0:3473b92e991e 1 #include "mbed.h"
skyyoungsik 0:3473b92e991e 2 #include <math.h>
skyyoungsik 1:9530746906b6 3 #include "Function.h"
skyyoungsik 0:3473b92e991e 4 #include "millis.h"
skyyoungsik 0:3473b92e991e 5 #include "SPATIAL.h"
skyyoungsik 0:3473b92e991e 6 #include "ROBOFRIEN_GUI.h"
skyyoungsik 0:3473b92e991e 7 #include "ROBOFRIEN_LED.h"
skyyoungsik 0:3473b92e991e 8 #include "ROBOFRIEN_MOTOR.h"
skyyoungsik 0:3473b92e991e 9 #include "ROBOFRIEN_PID.h"
skyyoungsik 1:9530746906b6 10 #include "ROBOFRIEN_SBUS.h"
skyyoungsik 1:9530746906b6 11 #include "ROBOFRIEN_BAT.h"
skyyoungsik 1:9530746906b6 12
skyyoungsik 1:9530746906b6 13
skyyoungsik 1:9530746906b6 14
skyyoungsik 1:9530746906b6 15 /* ************ USER INTERFACE ************** */
skyyoungsik 1:9530746906b6 16 #define SENSOR_WAIT 1
skyyoungsik 1:9530746906b6 17 #define THROTTLE_MAX 7000
skyyoungsik 1:9530746906b6 18 #define THROTTLE_BASE 4000
skyyoungsik 1:9530746906b6 19 #define THROTTLE_MIN 3000
skyyoungsik 1:9530746906b6 20 #define THROTTLE_CONTROL_LIMIT 1500
skyyoungsik 1:9530746906b6 21 #define LIMIT_PROGRESS_VELOCITY 10
skyyoungsik 1:9530746906b6 22 #define LIMIT_UP_VELOCITY 4
skyyoungsik 1:9530746906b6 23 #define LIMIT_DOWN_VELOCITY 2
skyyoungsik 1:9530746906b6 24 #define LIMIT_PLANE_VEL_INTEG 5 // Degree
skyyoungsik 1:9530746906b6 25 #define LIMIT_VERTICAL_VEL_INTEG 500
skyyoungsik 1:9530746906b6 26 /* ******************************************* */
skyyoungsik 1:9530746906b6 27
skyyoungsik 1:9530746906b6 28 /* *************************** */
skyyoungsik 1:9530746906b6 29 // AutoFlight Sequence
skyyoungsik 1:9530746906b6 30 // 0. Wait to Start
skyyoungsik 1:9530746906b6 31 // 1. Take Off (10m From Ground)
skyyoungsik 1:9530746906b6 32 // 2. Heading Align to Marker
skyyoungsik 1:9530746906b6 33 // 3. Go to Point ( Pitching & Altitude )
skyyoungsik 1:9530746906b6 34 // 4. Hovering
skyyoungsik 1:9530746906b6 35 // 5. Landing
skyyoungsik 1:9530746906b6 36 #define WAIT_TO_START 1
skyyoungsik 1:9530746906b6 37 #define TAKE_OFF 2
skyyoungsik 1:9530746906b6 38 #define HEADING_ALIGN 3
skyyoungsik 1:9530746906b6 39 #define GO_TO_POINT 4
skyyoungsik 1:9530746906b6 40 #define CHECK_MARKER 5
skyyoungsik 1:9530746906b6 41 #define LANDING 6
skyyoungsik 1:9530746906b6 42 #define WAIT 7
skyyoungsik 1:9530746906b6 43 int ex_AUTOFLIGHT_SEQUENCE;
skyyoungsik 1:9530746906b6 44 int AUTOFLIGHT_SEQUENCE = WAIT_TO_START;
skyyoungsik 1:9530746906b6 45 unsigned long AUTOFLIGHT_TIME_CNT;
skyyoungsik 1:9530746906b6 46 /* *************************** */
skyyoungsik 0:3473b92e991e 47
skyyoungsik 0:3473b92e991e 48 ROBOFRIEN_GUI GUI;
skyyoungsik 0:3473b92e991e 49 ROBOFRIEN_LED LED;
skyyoungsik 0:3473b92e991e 50 ROBOFRIEN_MOTOR MOTOR;
skyyoungsik 0:3473b92e991e 51 ROBOFRIEN_PID PID;
skyyoungsik 0:3473b92e991e 52 SPATIAL SPATIAL;
skyyoungsik 1:9530746906b6 53 ROBOFRIEN_SBUS SBUS;
skyyoungsik 1:9530746906b6 54 ROBOFRIEN_BAT BAT;
skyyoungsik 0:3473b92e991e 55
skyyoungsik 0:3473b92e991e 56
skyyoungsik 0:3473b92e991e 57 void GUI_UPDATE();
skyyoungsik 1:9530746906b6 58 void ALGORITHM();
skyyoungsik 1:9530746906b6 59 void ANGLE_CONTROL(double [3]);
skyyoungsik 1:9530746906b6 60 void PLANE_VEL_CONTROL(double, double, double *, double *);
skyyoungsik 1:9530746906b6 61 void VERTICAL_VEL_CONTROL(double, double *);
skyyoungsik 1:9530746906b6 62 void VEL_INTEG_RESET();
skyyoungsik 1:9530746906b6 63 float DEG_TO_METER(float , float , float , float );
skyyoungsik 0:3473b92e991e 64
skyyoungsik 1:9530746906b6 65 unsigned long millis_100hz, millis_10hz, millis_200hz;
skyyoungsik 1:9530746906b6 66 double want_Angle[3];
skyyoungsik 1:9530746906b6 67 double want_Rate[3];
skyyoungsik 1:9530746906b6 68 double err_Angle[3];
skyyoungsik 1:9530746906b6 69 double THROTTLE_VALUE;
skyyoungsik 1:9530746906b6 70 double want_LNG, want_LAT, want_ALT;
skyyoungsik 1:9530746906b6 71 int main()
skyyoungsik 0:3473b92e991e 72 {
skyyoungsik 1:9530746906b6 73 LED.Init();
skyyoungsik 1:9530746906b6 74 MOTOR.Init();
skyyoungsik 1:9530746906b6 75 wait(1);
skyyoungsik 1:9530746906b6 76 SPATIAL.Init(SENSOR_WAIT);
skyyoungsik 1:9530746906b6 77 SBUS.Init();
skyyoungsik 1:9530746906b6 78 PID.Init();
skyyoungsik 1:9530746906b6 79 GUI.Init();
skyyoungsik 1:9530746906b6 80 BAT.Init(3); // 3cell
skyyoungsik 1:9530746906b6 81 millisStart();
skyyoungsik 1:9530746906b6 82 while (1) {
skyyoungsik 1:9530746906b6 83 GUI.pc_rx_update();
skyyoungsik 1:9530746906b6 84 GUI.TRANS_BUFFER();
skyyoungsik 1:9530746906b6 85 LED.update(GUI.headlight_period, GUI.headlight_dutyrate, GUI.sidelight_period, GUI.sidelight_dutyrate );
skyyoungsik 1:9530746906b6 86 SPATIAL.SPATIAL_RECEIVE();
skyyoungsik 1:9530746906b6 87 SBUS.update();
skyyoungsik 1:9530746906b6 88 if ( ( millis() - millis_200hz ) > 5 ) {
skyyoungsik 1:9530746906b6 89 millis_200hz = millis();
skyyoungsik 1:9530746906b6 90 if ( SPATIAL.SPATIAL_STABLE() == false ) {
skyyoungsik 1:9530746906b6 91 LED.ALARM(1);
skyyoungsik 1:9530746906b6 92 if (GUI.button[0] == true) {
skyyoungsik 1:9530746906b6 93 ALGORITHM();
skyyoungsik 1:9530746906b6 94 MOTOR.update();
skyyoungsik 1:9530746906b6 95 } else {
skyyoungsik 1:9530746906b6 96 MOTOR.OFF();
skyyoungsik 0:3473b92e991e 97 }
skyyoungsik 1:9530746906b6 98 } else {
skyyoungsik 1:9530746906b6 99 LED.ALARM(0);
skyyoungsik 1:9530746906b6 100 ALGORITHM();
skyyoungsik 1:9530746906b6 101 MOTOR.update();
skyyoungsik 1:9530746906b6 102 }
skyyoungsik 0:3473b92e991e 103 }
skyyoungsik 1:9530746906b6 104
skyyoungsik 1:9530746906b6 105 if ( (millis() - millis_10hz ) > 100) {
skyyoungsik 1:9530746906b6 106 millis_10hz = millis();
skyyoungsik 1:9530746906b6 107 GUI_UPDATE();
skyyoungsik 1:9530746906b6 108 GUI.Trans();
skyyoungsik 1:9530746906b6 109 GUI.SET_DATA();
skyyoungsik 1:9530746906b6 110 }
skyyoungsik 1:9530746906b6 111 if (GUI.rx_bool() == true) {
skyyoungsik 1:9530746906b6 112 GUI.Refresh();
skyyoungsik 1:9530746906b6 113 }
skyyoungsik 1:9530746906b6 114 }
skyyoungsik 0:3473b92e991e 115 }
skyyoungsik 0:3473b92e991e 116
skyyoungsik 1:9530746906b6 117
skyyoungsik 1:9530746906b6 118 uint8_t EX_CONTROLLER_STATE;
skyyoungsik 1:9530746906b6 119 bool MOTOR_FORCED_OFF_BOOL = true;
skyyoungsik 1:9530746906b6 120 void ALGORITHM() { // 200 Hz //
skyyoungsik 1:9530746906b6 121 /* *************************** */
skyyoungsik 1:9530746906b6 122 /* ******* Want Angle ******** */
skyyoungsik 1:9530746906b6 123 /* *************************** */
skyyoungsik 1:9530746906b6 124 switch (SBUS.CONTROLLER_STATE) {
skyyoungsik 1:9530746906b6 125 case SIGNAL_OFF: {
skyyoungsik 1:9530746906b6 126 want_Angle[0] = 0; want_Angle[1] = 0; want_Angle[2] = SPATIAL.YAW;
skyyoungsik 1:9530746906b6 127 THROTTLE_VALUE = 0;
skyyoungsik 1:9530746906b6 128 } break;
skyyoungsik 1:9530746906b6 129 case MOTOR_OFF: {
skyyoungsik 1:9530746906b6 130 MOTOR_FORCED_OFF_BOOL = true;
skyyoungsik 1:9530746906b6 131 want_Angle[0] = 0; want_Angle[1] = 0; want_Angle[2] = SPATIAL.YAW;
skyyoungsik 1:9530746906b6 132 THROTTLE_VALUE = 0;
skyyoungsik 1:9530746906b6 133 } break;
skyyoungsik 1:9530746906b6 134 case MANUAL_ATTITUDE: {
skyyoungsik 1:9530746906b6 135 MOTOR_FORCED_OFF_BOOL = false;
skyyoungsik 1:9530746906b6 136 want_Angle[0] = ((SBUS.channel[0]) / 1000.0) * GUI.limit_rollx100 / 100.0; // Want Angle : Roll
skyyoungsik 1:9530746906b6 137 want_Angle[1] = ((SBUS.channel[1]) / 1000.0) * GUI.limit_pitchx100 / 100.0; // Want Angle : Pitch
skyyoungsik 1:9530746906b6 138 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);
skyyoungsik 1:9530746906b6 139 if ( want_Angle[2] > 360) want_Angle[2] -= 360;
skyyoungsik 1:9530746906b6 140 else if ( want_Angle[2] < 0) want_Angle[2] += 360;
skyyoungsik 1:9530746906b6 141 THROTTLE_VALUE = ((SBUS.channel[2] + 1000) / 2000.0 * THROTTLE_MAX);
skyyoungsik 1:9530746906b6 142 } break;
skyyoungsik 1:9530746906b6 143 case MANUAL_VELOCITY ... AUTO_FLIGHT: { // MANUAL_VELOCITY(3), MANUAL_POSITION(4), AUTO_FLIGHT(5) //
skyyoungsik 1:9530746906b6 144 //// [LNG]Longitude Up : East Face, Longitude Down : West Face
skyyoungsik 1:9530746906b6 145 //// [LAT]Latitude Up : North Face, Latitude Down : South Face
skyyoungsik 1:9530746906b6 146 if ( EX_CONTROLLER_STATE != SBUS.CONTROLLER_STATE) {
skyyoungsik 1:9530746906b6 147 want_LAT = SPATIAL.LATITUDE;
skyyoungsik 1:9530746906b6 148 want_LNG = SPATIAL.LONGITUDE;
skyyoungsik 1:9530746906b6 149 want_ALT = SPATIAL.HEIGHT;
skyyoungsik 1:9530746906b6 150 AUTOFLIGHT_TIME_CNT = 0;
skyyoungsik 1:9530746906b6 151 }
skyyoungsik 1:9530746906b6 152
skyyoungsik 1:9530746906b6 153 double want_Vel[3]; // VEL[0] = X-axis, VEL[1] = Y-axis, VEL[2] = Z-axis
skyyoungsik 1:9530746906b6 154 if ( SBUS.CONTROLLER_STATE == MANUAL_VELOCITY ) {
skyyoungsik 1:9530746906b6 155 /* ******************************************************* */
skyyoungsik 1:9530746906b6 156 /* ****************** MANUAL VELOCITY ******************** */
skyyoungsik 1:9530746906b6 157 /* ******************************************************* */
skyyoungsik 1:9530746906b6 158 MOTOR_FORCED_OFF_BOOL = false;
skyyoungsik 1:9530746906b6 159 want_Vel[0] = -(SBUS.channel[1] / 1000.0) * LIMIT_PROGRESS_VELOCITY;
skyyoungsik 1:9530746906b6 160 want_Vel[1] = (SBUS.channel[0] / 1000.0) * LIMIT_PROGRESS_VELOCITY;
skyyoungsik 1:9530746906b6 161 if ( SBUS.channel[2] >= 0 ) want_Vel[2] = (SBUS.channel[2] / 1000.0) * LIMIT_UP_VELOCITY;
skyyoungsik 1:9530746906b6 162 else if ( SBUS.channel[2] < 0 ) want_Vel[2] = (SBUS.channel[2] / 1000.0) * LIMIT_DOWN_VELOCITY;
skyyoungsik 1:9530746906b6 163 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);
skyyoungsik 1:9530746906b6 164
skyyoungsik 1:9530746906b6 165 } else {
skyyoungsik 1:9530746906b6 166 if ( SBUS.CONTROLLER_STATE == MANUAL_POSITION ) {
skyyoungsik 1:9530746906b6 167 /* ******************************************************* */
skyyoungsik 1:9530746906b6 168 /* ****************** MANUAL POSITION ******************** */
skyyoungsik 1:9530746906b6 169 /* ******************************************************* */
skyyoungsik 1:9530746906b6 170 MOTOR_FORCED_OFF_BOOL = false;
skyyoungsik 1:9530746906b6 171 double del_pos[3];
skyyoungsik 1:9530746906b6 172 del_pos[0] = (-(SBUS.channel[1] / 1000.0) * LIMIT_PROGRESS_VELOCITY); //divided by time
skyyoungsik 1:9530746906b6 173 del_pos[1] = ((SBUS.channel[0] / 1000.0) * LIMIT_PROGRESS_VELOCITY); //divided by time
skyyoungsik 1:9530746906b6 174 if (SBUS.channel[2] >= 0) del_pos[2] = ((SBUS.channel[2] / 1000.0) * LIMIT_UP_VELOCITY); //divided by time
skyyoungsik 1:9530746906b6 175 else if (SBUS.channel[2] < 0) del_pos[2] = ((SBUS.channel[2] / 1000.0) * LIMIT_DOWN_VELOCITY); //divided by time
skyyoungsik 1:9530746906b6 176
skyyoungsik 1:9530746906b6 177 double NED_del_pos[3];
skyyoungsik 1:9530746906b6 178 NED_del_pos[0] = del_pos[0] * cos(SPATIAL.YAW * M_PI / 180.0) - del_pos[1] * sin(SPATIAL.YAW * M_PI / 180.0);
skyyoungsik 1:9530746906b6 179 NED_del_pos[1] = del_pos[0] * sin(SPATIAL.YAW * M_PI / 180.0) + del_pos[1] * cos(SPATIAL.YAW * M_PI / 180.0);
skyyoungsik 1:9530746906b6 180
skyyoungsik 1:9530746906b6 181 if( (SBUS.channel[1] < 5) & (SBUS.channel[1] > -5) ) {}
skyyoungsik 1:9530746906b6 182 else want_LAT += (NED_del_pos[0] / 200.0) / 110574.0;
skyyoungsik 1:9530746906b6 183 if( (SBUS.channel[0] < 5) & (SBUS.channel[0] > -5) ) {}
skyyoungsik 1:9530746906b6 184 else want_LNG += (NED_del_pos[1] / 200.0) / 111320.0;
skyyoungsik 1:9530746906b6 185 if( (SBUS.channel[2] < 10) & (SBUS.channel[2] > -10) ) {}
skyyoungsik 1:9530746906b6 186 else want_ALT += (del_pos[2] / 200.0);
skyyoungsik 1:9530746906b6 187
skyyoungsik 1:9530746906b6 188 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);
skyyoungsik 1:9530746906b6 189
skyyoungsik 1:9530746906b6 190 } else if ( SBUS.CONTROLLER_STATE == AUTO_FLIGHT) {
skyyoungsik 1:9530746906b6 191 /* ******************************************************* */
skyyoungsik 1:9530746906b6 192 /* ****************** AUTO FLIGHT ******************** */
skyyoungsik 1:9530746906b6 193 /* ******************************************************* */
skyyoungsik 1:9530746906b6 194 if (AUTOFLIGHT_SEQUENCE != ex_AUTOFLIGHT_SEQUENCE) {
skyyoungsik 1:9530746906b6 195 AUTOFLIGHT_TIME_CNT = 0;
skyyoungsik 1:9530746906b6 196 }
skyyoungsik 1:9530746906b6 197 ex_AUTOFLIGHT_SEQUENCE = AUTOFLIGHT_SEQUENCE;
skyyoungsik 1:9530746906b6 198 AUTOFLIGHT_TIME_CNT ++;
skyyoungsik 1:9530746906b6 199 if ( AUTOFLIGHT_SEQUENCE == WAIT_TO_START) MOTOR_FORCED_OFF_BOOL = true;
skyyoungsik 1:9530746906b6 200 else MOTOR_FORCED_OFF_BOOL = false;
skyyoungsik 1:9530746906b6 201 switch (AUTOFLIGHT_SEQUENCE) {
skyyoungsik 1:9530746906b6 202 case WAIT_TO_START: {
skyyoungsik 1:9530746906b6 203 if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) AUTOFLIGHT_SEQUENCE = TAKE_OFF;
skyyoungsik 1:9530746906b6 204 want_LAT = SPATIAL.LATITUDE;
skyyoungsik 1:9530746906b6 205 want_LNG = SPATIAL.LONGITUDE;
skyyoungsik 1:9530746906b6 206 want_ALT = SPATIAL.HEIGHT;
skyyoungsik 1:9530746906b6 207 want_Angle[2] = SPATIAL.YAW;
skyyoungsik 1:9530746906b6 208 break;
skyyoungsik 1:9530746906b6 209 }
skyyoungsik 1:9530746906b6 210 case TAKE_OFF: { /// Take off [ Ground --> 5m ] in 1m/s
skyyoungsik 1:9530746906b6 211 want_ALT += (1 / 200.0);
skyyoungsik 1:9530746906b6 212 if ( AUTOFLIGHT_TIME_CNT > (5 * 200)) {
skyyoungsik 1:9530746906b6 213 AUTOFLIGHT_SEQUENCE = CHECK_MARKER;
skyyoungsik 1:9530746906b6 214 GUI.CurrentMarker = 1;
skyyoungsik 1:9530746906b6 215 }
skyyoungsik 1:9530746906b6 216 break;
skyyoungsik 1:9530746906b6 217 }
skyyoungsik 1:9530746906b6 218 case HEADING_ALIGN: {
skyyoungsik 1:9530746906b6 219 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);
skyyoungsik 1:9530746906b6 220 want_Angle[2] = BEARING_ANGLE;
skyyoungsik 1:9530746906b6 221 if ( ABSOL((BEARING_ANGLE - SPATIAL.YAW)) < 5 ) {
skyyoungsik 1:9530746906b6 222 if (AUTOFLIGHT_TIME_CNT > (3 * 200) ) AUTOFLIGHT_SEQUENCE = GO_TO_POINT;
skyyoungsik 1:9530746906b6 223 } else AUTOFLIGHT_TIME_CNT = 0;
skyyoungsik 1:9530746906b6 224 // GUI.DEBUGx100[0] = (BEARING_ANGLE + 100) * 100;
skyyoungsik 1:9530746906b6 225 // GUI.DEBUGx100[1] = (ABSOL((BEARING_ANGLE - SPATIAL.YAW)) + 100) * 100;
skyyoungsik 1:9530746906b6 226 break;
skyyoungsik 1:9530746906b6 227 }
skyyoungsik 1:9530746906b6 228 case GO_TO_POINT: {
skyyoungsik 1:9530746906b6 229 float Target_LAT = GUI.Marker_Lat[GUI.CurrentMarker - 1] / 1000000.0;
skyyoungsik 1:9530746906b6 230 float Target_LNG = GUI.Marker_Lng[GUI.CurrentMarker - 1] / 1000000.0;
skyyoungsik 1:9530746906b6 231 float Target_ALT = GUI.Marker_Alt[GUI.CurrentMarker - 1] / 100.0;
skyyoungsik 1:9530746906b6 232 // 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);
skyyoungsik 1:9530746906b6 233 // want_Angle[2] = BEARING_ANGLE;
skyyoungsik 1:9530746906b6 234 // 1ms ++
skyyoungsik 1:9530746906b6 235 /* if(Target_LAT > want_LAT) want_LAT += (1.5/200.0)/110574.0;
skyyoungsik 1:9530746906b6 236 else want_LAT -= (1/200.0)/110574.0;
skyyoungsik 1:9530746906b6 237 if(Target_LNG > want_LNG) want_LNG += (1.5/200.0)/111320.0;
skyyoungsik 1:9530746906b6 238 else want_LNG -= (1/200.0)/111320.0;
skyyoungsik 1:9530746906b6 239 if(Target_ALT > want_ALT) want_ALT += (1/200.0);
skyyoungsik 1:9530746906b6 240 else want_ALT -= (1/200.0);
skyyoungsik 1:9530746906b6 241 */
skyyoungsik 1:9530746906b6 242 // want_LAT = Target_LAT;
skyyoungsik 1:9530746906b6 243 // want_LNG = Target_LNG;
skyyoungsik 1:9530746906b6 244 // want_ALT = Target_ALT;
skyyoungsik 1:9530746906b6 245 if(Target_LAT > want_LAT) want_LAT += (LIMIT_PROGRESS_VELOCITY/200.0)/110574.0;
skyyoungsik 1:9530746906b6 246 else want_LAT -= (LIMIT_PROGRESS_VELOCITY/200.0)/110574.0;
skyyoungsik 1:9530746906b6 247 if(Target_LNG > want_LNG) want_LNG += (LIMIT_PROGRESS_VELOCITY/200.0)/111320.0;
skyyoungsik 1:9530746906b6 248 else want_LNG -= (LIMIT_PROGRESS_VELOCITY/200.0)/111320.0;
skyyoungsik 1:9530746906b6 249 if(Target_ALT > want_ALT) want_ALT += (LIMIT_UP_VELOCITY/200.0);
skyyoungsik 1:9530746906b6 250 else want_ALT -= (LIMIT_DOWN_VELOCITY/200.0);
skyyoungsik 1:9530746906b6 251
skyyoungsik 1:9530746906b6 252 bool escape_bool = false;
skyyoungsik 1:9530746906b6 253 float POSITION_DISTANCE;
skyyoungsik 1:9530746906b6 254 float HEIGHT_DISTANCE;
skyyoungsik 1:9530746906b6 255 POSITION_DISTANCE = DEG_TO_METER( want_LAT, want_LNG, SPATIAL.LATITUDE, SPATIAL.LONGITUDE );
skyyoungsik 1:9530746906b6 256 HEIGHT_DISTANCE = sqrt( (want_ALT - SPATIAL.HEIGHT) * (want_ALT - SPATIAL.HEIGHT) );
skyyoungsik 1:9530746906b6 257 if ( ( POSITION_DISTANCE < 5 ) & (HEIGHT_DISTANCE < 5)) { // 5m
skyyoungsik 1:9530746906b6 258 if ( (POSITION_DISTANCE < 3) ) { // 3m
skyyoungsik 1:9530746906b6 259 if ( (POSITION_DISTANCE < 1) ) { // 1m
skyyoungsik 1:9530746906b6 260 ///////////// Target in the 1m OK !!!! /////////////////
skyyoungsik 1:9530746906b6 261 escape_bool = true;
skyyoungsik 1:9530746906b6 262 /////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 263 }
skyyoungsik 1:9530746906b6 264 else {
skyyoungsik 1:9530746906b6 265 if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) escape_bool = true; // Time Out //
skyyoungsik 1:9530746906b6 266 }
skyyoungsik 1:9530746906b6 267 }
skyyoungsik 1:9530746906b6 268 else {
skyyoungsik 1:9530746906b6 269 if ( AUTOFLIGHT_TIME_CNT > (5 * 200) ) escape_bool = true; // Time Out //
skyyoungsik 1:9530746906b6 270 }
skyyoungsik 1:9530746906b6 271 }
skyyoungsik 1:9530746906b6 272 else {
skyyoungsik 1:9530746906b6 273 AUTOFLIGHT_TIME_CNT = 0;
skyyoungsik 1:9530746906b6 274 }
skyyoungsik 1:9530746906b6 275 if ( escape_bool == true) {
skyyoungsik 1:9530746906b6 276 AUTOFLIGHT_SEQUENCE = CHECK_MARKER;
skyyoungsik 1:9530746906b6 277 GUI.CurrentMarker ++;
skyyoungsik 1:9530746906b6 278 }
skyyoungsik 1:9530746906b6 279 // 1m내외
skyyoungsik 1:9530746906b6 280 break;
skyyoungsik 1:9530746906b6 281 }
skyyoungsik 1:9530746906b6 282 case CHECK_MARKER: {
skyyoungsik 1:9530746906b6 283 if (AUTOFLIGHT_TIME_CNT > (3 * 200) ) {
skyyoungsik 1:9530746906b6 284 if ( (GUI.Marker_Lat[GUI.CurrentMarker - 1] != 0) & (GUI.Marker_Lng[GUI.CurrentMarker - 1] != 0) ) {
skyyoungsik 1:9530746906b6 285 AUTOFLIGHT_SEQUENCE = HEADING_ALIGN;
skyyoungsik 1:9530746906b6 286 } else {
skyyoungsik 1:9530746906b6 287 AUTOFLIGHT_SEQUENCE = LANDING;
skyyoungsik 1:9530746906b6 288 }
skyyoungsik 1:9530746906b6 289 }
skyyoungsik 1:9530746906b6 290 break;
skyyoungsik 1:9530746906b6 291 }
skyyoungsik 1:9530746906b6 292 case LANDING: {
skyyoungsik 1:9530746906b6 293 want_ALT -= (0.5 / 200.0); // 0.5m/s
skyyoungsik 1:9530746906b6 294 break;
skyyoungsik 1:9530746906b6 295 }
skyyoungsik 1:9530746906b6 296 case WAIT: {
skyyoungsik 1:9530746906b6 297 break;
skyyoungsik 1:9530746906b6 298 }
skyyoungsik 1:9530746906b6 299 }
skyyoungsik 1:9530746906b6 300 }
skyyoungsik 1:9530746906b6 301 double NED_want_vel[3];
skyyoungsik 1:9530746906b6 302 NED_want_vel[0] = (want_LAT - SPATIAL.LATITUDE) * 110574.0 * (GUI.gain_px100[4] / 100.0);
skyyoungsik 1:9530746906b6 303 NED_want_vel[1] = (want_LNG - SPATIAL.LONGITUDE) * 111320.0 * (GUI.gain_px100[5] / 100.0);
skyyoungsik 1:9530746906b6 304 NED_want_vel[2] = (want_ALT - SPATIAL.HEIGHT) * (GUI.gain_px100[6] / 100.0);
skyyoungsik 1:9530746906b6 305
skyyoungsik 1:9530746906b6 306 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);
skyyoungsik 1:9530746906b6 307 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);
skyyoungsik 1:9530746906b6 308 want_Vel[2] = NED_want_vel[2];
skyyoungsik 1:9530746906b6 309
skyyoungsik 1:9530746906b6 310 float LIMIT_PROGRESS_VELOCITY_CALCULATED;
skyyoungsik 1:9530746906b6 311 float POSITION_DISTANCE = DEG_TO_METER( want_LAT, want_LNG, SPATIAL.LATITUDE, SPATIAL.LONGITUDE );
skyyoungsik 1:9530746906b6 312 if ( POSITION_DISTANCE < 20) LIMIT_PROGRESS_VELOCITY_CALCULATED = (0.04 * POSITION_DISTANCE + 0.2) * LIMIT_PROGRESS_VELOCITY;
skyyoungsik 1:9530746906b6 313 else LIMIT_PROGRESS_VELOCITY_CALCULATED = LIMIT_PROGRESS_VELOCITY;
skyyoungsik 1:9530746906b6 314
skyyoungsik 1:9530746906b6 315 want_Vel[0] = MIN_MAX(want_Vel[0], -LIMIT_PROGRESS_VELOCITY_CALCULATED, LIMIT_PROGRESS_VELOCITY_CALCULATED);
skyyoungsik 1:9530746906b6 316 want_Vel[1] = MIN_MAX(want_Vel[1], -LIMIT_PROGRESS_VELOCITY_CALCULATED, LIMIT_PROGRESS_VELOCITY_CALCULATED);
skyyoungsik 1:9530746906b6 317 if (want_Vel[2] >= 0) want_Vel[2] = MIN_MAX(want_Vel[2], -LIMIT_UP_VELOCITY, LIMIT_UP_VELOCITY);
skyyoungsik 1:9530746906b6 318 else if (want_Vel[2] < 0) want_Vel[2] = MIN_MAX(want_Vel[2], -LIMIT_DOWN_VELOCITY, LIMIT_DOWN_VELOCITY);
skyyoungsik 1:9530746906b6 319
skyyoungsik 1:9530746906b6 320 }
skyyoungsik 1:9530746906b6 321
skyyoungsik 1:9530746906b6 322 /* ********** WANT ANGLE GENERATE ************ */
skyyoungsik 1:9530746906b6 323 PLANE_VEL_CONTROL(want_Vel[0], want_Vel[1], &want_Angle[0], &want_Angle[1]);
skyyoungsik 1:9530746906b6 324 if ( want_Angle[2] > 360) want_Angle[2] -= 360;
skyyoungsik 1:9530746906b6 325 else if ( want_Angle[2] < 0) want_Angle[2] += 360;
skyyoungsik 1:9530746906b6 326
skyyoungsik 1:9530746906b6 327 /* ********** THROTTLE VALUE GENRATE ************* */
skyyoungsik 1:9530746906b6 328 VERTICAL_VEL_CONTROL(want_Vel[2], &THROTTLE_VALUE);
skyyoungsik 1:9530746906b6 329 if (MOTOR_FORCED_OFF_BOOL == true) THROTTLE_VALUE = 0;
skyyoungsik 1:9530746906b6 330
skyyoungsik 1:9530746906b6 331 } break;
skyyoungsik 1:9530746906b6 332 }
skyyoungsik 1:9530746906b6 333 EX_CONTROLLER_STATE = SBUS.CONTROLLER_STATE;
skyyoungsik 1:9530746906b6 334 /* ************************** */
skyyoungsik 1:9530746906b6 335 /* ******* Want Rate ******** */
skyyoungsik 1:9530746906b6 336 /* ************************** */
skyyoungsik 1:9530746906b6 337 ANGLE_CONTROL( want_Angle );
skyyoungsik 1:9530746906b6 338
skyyoungsik 1:9530746906b6 339 if (GUI.DPN_Info == 0) {
skyyoungsik 1:9530746906b6 340 if ( SBUS.CONTROLLER_STATE <= MOTOR_OFF) {
skyyoungsik 1:9530746906b6 341 PID.RAT_INTEGER_RESET();
skyyoungsik 1:9530746906b6 342 VEL_INTEG_RESET();
skyyoungsik 1:9530746906b6 343 for (int i = 0; i < 6; i++) MOTOR.Value[i] = 0;
skyyoungsik 1:9530746906b6 344 } else {
skyyoungsik 1:9530746906b6 345 int MOTOR_MIN_DEADZONE = 5;
skyyoungsik 1:9530746906b6 346 for (int i = 0; i < 5; i++) {
skyyoungsik 1:9530746906b6 347 if (GUI.motor_min[i] > 100)GUI.motor_min[i] = 100;
skyyoungsik 1:9530746906b6 348 }
skyyoungsik 1:9530746906b6 349 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];
skyyoungsik 1:9530746906b6 350 MOTOR.Value[1] = (GUI.motor_min[1] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE + PID.output_Data[0] + 0 + PID.output_Data[2];
skyyoungsik 1:9530746906b6 351 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];
skyyoungsik 1:9530746906b6 352 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];
skyyoungsik 1:9530746906b6 353 MOTOR.Value[4] = (GUI.motor_min[4] - MOTOR_MIN_DEADZONE) * 10 + THROTTLE_VALUE - PID.output_Data[0] + 0 - PID.output_Data[2];
skyyoungsik 1:9530746906b6 354 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];
skyyoungsik 1:9530746906b6 355 for (int i = 0; i < 6; i++) {
skyyoungsik 1:9530746906b6 356 if ( MOTOR.Value[i] < ((GUI.motor_min[0] - MOTOR_MIN_DEADZONE) * 10 ) ) {
skyyoungsik 1:9530746906b6 357 MOTOR.Value[i] = ((GUI.motor_min[0] - MOTOR_MIN_DEADZONE) * 10 );
skyyoungsik 1:9530746906b6 358 }
skyyoungsik 1:9530746906b6 359 }
skyyoungsik 0:3473b92e991e 360 }
skyyoungsik 1:9530746906b6 361 } else if (GUI.DPN_Info == 4) {
skyyoungsik 1:9530746906b6 362 MOTOR.Value[0] = (float)GUI.pwm_info[0] * 10;
skyyoungsik 1:9530746906b6 363 MOTOR.Value[1] = (float)GUI.pwm_info[1] * 10;
skyyoungsik 1:9530746906b6 364 MOTOR.Value[2] = (float)GUI.pwm_info[2] * 10;
skyyoungsik 1:9530746906b6 365 MOTOR.Value[3] = (float)GUI.pwm_info[3] * 10;
skyyoungsik 1:9530746906b6 366 MOTOR.Value[4] = (float)GUI.pwm_info[4] * 10;
skyyoungsik 1:9530746906b6 367 MOTOR.Value[5] = (float)GUI.pwm_info[5] * 10;
skyyoungsik 1:9530746906b6 368 }
skyyoungsik 1:9530746906b6 369 }
skyyoungsik 1:9530746906b6 370
skyyoungsik 1:9530746906b6 371
skyyoungsik 1:9530746906b6 372 void GUI_UPDATE() {
skyyoungsik 1:9530746906b6 373 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 374 //////////////////////////////////////////// [M/S] Mode and State ///////////////////////////////////////
skyyoungsik 1:9530746906b6 375 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 376 /* ************ MODE 1 ************* */
skyyoungsik 1:9530746906b6 377 GUI.Mode1 = SBUS.CONTROLLER_STATE;
skyyoungsik 1:9530746906b6 378 GUI.Alarm = !SPATIAL.SPATIAL_STABLE();
skyyoungsik 1:9530746906b6 379
skyyoungsik 1:9530746906b6 380 GUI.MissionState = AUTOFLIGHT_SEQUENCE;
skyyoungsik 1:9530746906b6 381 GUI.Bat1 = BAT.update();
skyyoungsik 1:9530746906b6 382 // [ Button State & Home Point Chksum & Marker Chksum ] is change automatically in "ROBOFRIEN_GUI.cpp" //
skyyoungsik 1:9530746906b6 383 // You Can not change this setting, just use it ///
skyyoungsik 1:9530746906b6 384 if (GUI.button[0] == true) {}
skyyoungsik 1:9530746906b6 385 else {}
skyyoungsik 1:9530746906b6 386 if (GUI.button[1] == true) {
skyyoungsik 1:9530746906b6 387 AUTOFLIGHT_SEQUENCE = WAIT_TO_START;
skyyoungsik 1:9530746906b6 388 }
skyyoungsik 1:9530746906b6 389 else {}
skyyoungsik 1:9530746906b6 390 if (GUI.button[2] == true) {}
skyyoungsik 1:9530746906b6 391 else {}
skyyoungsik 1:9530746906b6 392 if (GUI.button[3] == true) {}
skyyoungsik 1:9530746906b6 393 else {}
skyyoungsik 1:9530746906b6 394 if (GUI.button[4] == true) {}
skyyoungsik 1:9530746906b6 395 else {}
skyyoungsik 1:9530746906b6 396
skyyoungsik 1:9530746906b6 397 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 398 //////////////////////////////////////////////// [GPS] GPS //////////////////////////////////////////////
skyyoungsik 1:9530746906b6 399 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 400 GUI.utc_time = SPATIAL.UNIX_TIME;
skyyoungsik 1:9530746906b6 401 GUI.latitude = (SPATIAL.LATITUDE * 1000000);
skyyoungsik 1:9530746906b6 402 GUI.longitude = (SPATIAL.LONGITUDE * 1000000);
skyyoungsik 1:9530746906b6 403 GUI.altitude = SPATIAL.HEIGHT * 100;
skyyoungsik 1:9530746906b6 404 GUI.SatNum = SPATIAL.SATELLITE_NUM;
skyyoungsik 1:9530746906b6 405 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 406 ///////////////////////////////////////////////// AHRS //////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 407 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 408 GUI.rollx100 = (SPATIAL.ROLL * 100);
skyyoungsik 1:9530746906b6 409 GUI.pitchx100 = (SPATIAL.PITCH * 100);
skyyoungsik 1:9530746906b6 410 GUI.yawx100 = ((SPATIAL.YAW ) * 100);
skyyoungsik 1:9530746906b6 411 GUI.roll_ratex100 = SPATIAL.ROLL_RATE * 100;
skyyoungsik 1:9530746906b6 412 GUI.pitch_ratex100 = SPATIAL.PITCH_RATE * 100;
skyyoungsik 1:9530746906b6 413 GUI.yaw_ratex100 = SPATIAL.YAW_RATE * 100;
skyyoungsik 1:9530746906b6 414 GUI.VXx100 = SPATIAL.Vx * 100;
skyyoungsik 1:9530746906b6 415 GUI.VYx100 = SPATIAL.Vy * 100;
skyyoungsik 1:9530746906b6 416 GUI.VZx100 = SPATIAL.Vz * 100;
skyyoungsik 1:9530746906b6 417
skyyoungsik 1:9530746906b6 418 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 419 ////////////////////////////////////////////// [C/P] CAP/PWM ////////////////////////////////////////////
skyyoungsik 1:9530746906b6 420 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 421
skyyoungsik 1:9530746906b6 422 GUI.cap[0] = SBUS.channel[0] / 10.0;
skyyoungsik 1:9530746906b6 423 GUI.cap[1] = SBUS.channel[1] / 10.0;
skyyoungsik 1:9530746906b6 424 GUI.cap[2] = SBUS.channel[2] / 10.0;
skyyoungsik 1:9530746906b6 425 GUI.cap[3] = SBUS.channel[3] / 10.0;
skyyoungsik 1:9530746906b6 426 GUI.cap[4] = SBUS.channel[4] / 10.0;
skyyoungsik 1:9530746906b6 427 GUI.cap[5] = SBUS.channel[5] / 10.0;
skyyoungsik 1:9530746906b6 428 GUI.cap[6] = SBUS.channel[6] / 10.0;
skyyoungsik 1:9530746906b6 429 GUI.cap[7] = SBUS.channel[7] / 10.0;
skyyoungsik 1:9530746906b6 430
skyyoungsik 1:9530746906b6 431 GUI.pwm[0] = MOTOR.Value[0] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
skyyoungsik 1:9530746906b6 432 GUI.pwm[1] = MOTOR.Value[1] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
skyyoungsik 1:9530746906b6 433 GUI.pwm[2] = MOTOR.Value[2] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
skyyoungsik 1:9530746906b6 434 GUI.pwm[3] = MOTOR.Value[3] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
skyyoungsik 1:9530746906b6 435 GUI.pwm[4] = MOTOR.Value[4] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
skyyoungsik 1:9530746906b6 436 GUI.pwm[5] = MOTOR.Value[5] / 100; // (0 ~ 10000) --> ( 0 ~ 100 )
skyyoungsik 1:9530746906b6 437
skyyoungsik 1:9530746906b6 438 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 439 /////////////////////////////////////////// [E/D] Extra & Debug /////////////////////////////////////////
skyyoungsik 1:9530746906b6 440 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 441
skyyoungsik 1:9530746906b6 442 GUI.DEBUGx100[2] = (want_Angle[2] + 100) * 100;
skyyoungsik 1:9530746906b6 443 GUI.DEBUGx100[3] = GUI.Controller_Joystick[3];
skyyoungsik 1:9530746906b6 444 GUI.DEBUGx100[4] = GUI.Gimbal_Joystick[0];
skyyoungsik 1:9530746906b6 445 GUI.DEBUGx100[5] = GUI.Gimbal_Joystick[1];
skyyoungsik 1:9530746906b6 446 float PNU_RPM[2];
skyyoungsik 1:9530746906b6 447 PNU_RPM[0] = sin( millis() / 10000.0 ) * 10000;
skyyoungsik 1:9530746906b6 448 PNU_RPM[1] = cos( millis() / 10000.0 ) * 10000;
skyyoungsik 1:9530746906b6 449 GUI.DEBUGx100[6] = PNU_RPM[0];
skyyoungsik 1:9530746906b6 450 GUI.DEBUGx100[7] = PNU_RPM[1];
skyyoungsik 1:9530746906b6 451
skyyoungsik 1:9530746906b6 452 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 453 /////////////////////////////////////////// Configuration ///////////////////////////////////////////////
skyyoungsik 1:9530746906b6 454 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 455
skyyoungsik 1:9530746906b6 456 ///////////////////////////// -----------------DPN 1-------------------- ////////////////////////////////
skyyoungsik 1:9530746906b6 457 // You can set the this value from "Config.h" ( EEPROM_MODEL_TYPE1, EEPROM_MODEL_TYPE2_UP, EEPROM_MODEL_TYPE2_DOWN ) //
skyyoungsik 1:9530746906b6 458
skyyoungsik 1:9530746906b6 459 ///////////////////////////// -----------------DPN 2-------------------- ////////////////////////////////
skyyoungsik 1:9530746906b6 460 // You can set the this value from "Config.h" ( MODEL_INFO, FIRMWARE_INFO ) //
skyyoungsik 1:9530746906b6 461
skyyoungsik 1:9530746906b6 462 ///////////////////////////// -----------------DPN 3-------------------- ////////////////////////////////
skyyoungsik 1:9530746906b6 463 /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) ///
skyyoungsik 1:9530746906b6 464 /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value //
skyyoungsik 1:9530746906b6 465
skyyoungsik 1:9530746906b6 466 ///////////////////////////// -----------------DPN 4-------------------- ////////////////////////////////
skyyoungsik 1:9530746906b6 467 /// You can use the [pwm_info , motor_min[4]] for calibration of motor pwm value //
skyyoungsik 1:9530746906b6 468
skyyoungsik 1:9530746906b6 469 ///////////////////////////// -----------------DPN 5-------------------- ////////////////////////////////
skyyoungsik 1:9530746906b6 470 /*
skyyoungsik 1:9530746906b6 471 LED.HeadlightPeriod = GUI.headlight_period;
skyyoungsik 1:9530746906b6 472 LED.HeadlightDutyrate = GUI.headlight_dutyrate;
skyyoungsik 1:9530746906b6 473 LED.SidelightPeriod = GUI.sidelight_period;
skyyoungsik 1:9530746906b6 474 LED.SidelightDutyrate = GUI.sidelight_dutyrate;
skyyoungsik 1:9530746906b6 475 */
skyyoungsik 1:9530746906b6 476 ///////////////////////////// -----------------DPN 6-------------------- ////////////////////////////////
skyyoungsik 1:9530746906b6 477 /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) ///
skyyoungsik 1:9530746906b6 478 /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value //
skyyoungsik 1:9530746906b6 479 GUI.raw_cap[0] = (0 - 150) * 2;
skyyoungsik 1:9530746906b6 480 GUI.raw_cap[1] = (0 - 150) * 2;
skyyoungsik 1:9530746906b6 481 GUI.raw_cap[2] = (0 - 150) * 2;
skyyoungsik 1:9530746906b6 482 GUI.raw_cap[3] = (0 - 150) * 2;
skyyoungsik 1:9530746906b6 483 GUI.raw_cap[4] = (0 - 150) * 2;
skyyoungsik 1:9530746906b6 484 GUI.raw_cap[5] = (0 - 150) * 2;
skyyoungsik 1:9530746906b6 485 GUI.raw_cap[6] = 0;
skyyoungsik 1:9530746906b6 486 GUI.raw_cap[7] = 0;
skyyoungsik 1:9530746906b6 487
skyyoungsik 1:9530746906b6 488 if (GUI.attitude_configuration_bool == true) {
skyyoungsik 1:9530746906b6 489 GUI.attitude_configuration_bool = false;
skyyoungsik 1:9530746906b6 490 // You can calibration of attitude (Roll, Pitch) //
skyyoungsik 1:9530746906b6 491 // GUI.attitude_calibrate(accelData[0]*aRes,accelData[1]*aRes,(accelData[2]*aRes-1));
skyyoungsik 1:9530746906b6 492 }
skyyoungsik 1:9530746906b6 493 if (GUI.Compass_Calibration_Mode == 1) {
skyyoungsik 1:9530746906b6 494 // You can calibration of compass (Yaw)//
skyyoungsik 0:3473b92e991e 495 // if(ex_Compass_Calibration_Mode == 0){
skyyoungsik 1:9530746906b6 496 // magnetic_offset_reset();
skyyoungsik 0:3473b92e991e 497 // }else{
skyyoungsik 0:3473b92e991e 498 // //// calibrating ... /////
skyyoungsik 1:9530746906b6 499 // magnetic_calibration();
skyyoungsik 0:3473b92e991e 500 // }
skyyoungsik 1:9530746906b6 501 } else if (GUI.Compass_Calibration_Mode == 2) {
skyyoungsik 1:9530746906b6 502 // // You can finish the calibration of compass
skyyoungsik 0:3473b92e991e 503 // //// write to eeprom ////
skyyoungsik 0:3473b92e991e 504 // GUI.Compass_Calibration_Mode = 0;
skyyoungsik 1:9530746906b6 505 // GUI.write_compass_setting_to_eeprom();
skyyoungsik 1:9530746906b6 506 }
skyyoungsik 1:9530746906b6 507 else if (GUI.Compass_Calibration_Mode == 3) { // declination angle
skyyoungsik 1:9530746906b6 508 SPATIAL.DECLINATION_ANGLE = GUI.declination_angle;
skyyoungsik 1:9530746906b6 509 }
skyyoungsik 1:9530746906b6 510 // ex_Compass_Calibration_Mode = GUI.Compass_Calibration_Mode;
skyyoungsik 1:9530746906b6 511
skyyoungsik 1:9530746906b6 512 ///////////////////////////// -----------------DPN 7-------------------- ////////////////////////////////
skyyoungsik 1:9530746906b6 513 /// You can use the [limit_rollx100, limit_pitchx100, limit_roll_ratex100, limit_pitch_ratex100, limit_yaw_ratex100] for limit the angle and angular velocity //
skyyoungsik 1:9530746906b6 514
skyyoungsik 1:9530746906b6 515 ///////////////////////////// -----------------DPN 8-------------------- ////////////////////////////////
skyyoungsik 1:9530746906b6 516 /// You can use the [gain_px100[20], gain_dx100[20], gain_ix100[20]] for gain tuning //
skyyoungsik 1:9530746906b6 517 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 518 //////////////////////////////////////////////// Refresh ////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 519 /////////////////////////////////////////////////////////////////////////////////////////////////////////
skyyoungsik 1:9530746906b6 520 // GUI.trans_flight_data(255, 0);
skyyoungsik 1:9530746906b6 521 GUI.pc_rx_update();
skyyoungsik 1:9530746906b6 522 }
skyyoungsik 1:9530746906b6 523
skyyoungsik 1:9530746906b6 524 void ANGLE_CONTROL(double IN_WANT_ANGLE[3]) {
skyyoungsik 1:9530746906b6 525 /* ********* MIN - MAX ************* */
skyyoungsik 1:9530746906b6 526 IN_WANT_ANGLE[0] = MIN_MAX(IN_WANT_ANGLE[0], -GUI.limit_rollx100 / 100.0, GUI.limit_rollx100 / 100.0 );
skyyoungsik 1:9530746906b6 527 IN_WANT_ANGLE[1] = MIN_MAX(IN_WANT_ANGLE[1], -GUI.limit_pitchx100 / 100.0, GUI.limit_pitchx100 / 100.0 );
skyyoungsik 1:9530746906b6 528 err_Angle[0] = IN_WANT_ANGLE[0] - SPATIAL.ROLL;
skyyoungsik 1:9530746906b6 529 err_Angle[1] = IN_WANT_ANGLE[1] - SPATIAL.PITCH;
skyyoungsik 1:9530746906b6 530 err_Angle[2] = IN_WANT_ANGLE[2] - SPATIAL.YAW;
skyyoungsik 1:9530746906b6 531 if (err_Angle[2] > 180) err_Angle[2] -= 360;
skyyoungsik 1:9530746906b6 532 else if (err_Angle[2] < -180) err_Angle[2] += 360;
skyyoungsik 1:9530746906b6 533 want_Rate[0] = err_Angle[0] * GUI.gain_px100[0] / 100.0; // Want Rate : Roll Rate
skyyoungsik 1:9530746906b6 534 want_Rate[1] = err_Angle[1] * GUI.gain_dx100[0] / 100.0; // Want Rate : Pitch Rate
skyyoungsik 1:9530746906b6 535 want_Rate[2] = err_Angle[2] * GUI.gain_ix100[0] / 100.0; // Want Rate : Yaw Rate
skyyoungsik 1:9530746906b6 536 want_Rate[0] = MIN_MAX(want_Rate[0], -GUI.limit_roll_ratex100 / 100.0, GUI.limit_roll_ratex100 / 100.0);
skyyoungsik 1:9530746906b6 537 want_Rate[1] = MIN_MAX(want_Rate[1], -GUI.limit_pitch_ratex100 / 100.0, GUI.limit_pitch_ratex100 / 100.0);
skyyoungsik 1:9530746906b6 538 want_Rate[2] = MIN_MAX(want_Rate[2], -GUI.limit_yaw_ratex100 / 100.0, GUI.limit_yaw_ratex100 / 100.0);
skyyoungsik 1:9530746906b6 539 PID.err_Rate[0] = (want_Rate[0] - SPATIAL.ROLL_RATE);
skyyoungsik 1:9530746906b6 540 PID.err_Rate[1] = (want_Rate[1] - SPATIAL.PITCH_RATE);
skyyoungsik 1:9530746906b6 541 PID.err_Rate[2] = (want_Rate[2] - SPATIAL.YAW_RATE);
skyyoungsik 1:9530746906b6 542 PID.RAT_KP[0] = -GUI.gain_px100[1] / 100.0;
skyyoungsik 1:9530746906b6 543 PID.RAT_KP[1] = -GUI.gain_px100[2] / 100.0;
skyyoungsik 1:9530746906b6 544 PID.RAT_KP[2] = -GUI.gain_px100[3] / 100.0;
skyyoungsik 1:9530746906b6 545 PID.RAT_KI[0] = -GUI.gain_ix100[1] / 100.0;
skyyoungsik 1:9530746906b6 546 PID.RAT_KI[1] = -GUI.gain_ix100[2] / 100.0;
skyyoungsik 1:9530746906b6 547 PID.RAT_KI[2] = -GUI.gain_ix100[3] / 100.0;
skyyoungsik 1:9530746906b6 548 PID.update();
skyyoungsik 0:3473b92e991e 549 }
skyyoungsik 1:9530746906b6 550
skyyoungsik 1:9530746906b6 551 double PLANE_VEL_INTEG[2];
skyyoungsik 1:9530746906b6 552 void PLANE_VEL_CONTROL(double IN_VEL_X, double IN_VEL_Y, double *OUTPUT_WANT_ROLL, double *OUTPUT_WANT_PITCH) {
skyyoungsik 1:9530746906b6 553 double err_Vel[2];
skyyoungsik 1:9530746906b6 554 err_Vel[0] = ( IN_VEL_X - SPATIAL.Vx );
skyyoungsik 1:9530746906b6 555 err_Vel[1] = ( IN_VEL_Y - SPATIAL.Vy );
skyyoungsik 1:9530746906b6 556 err_Vel[0] = MIN_MAX(err_Vel[0], -LIMIT_PROGRESS_VELOCITY, LIMIT_PROGRESS_VELOCITY);
skyyoungsik 1:9530746906b6 557 err_Vel[1] = MIN_MAX(err_Vel[1], -LIMIT_PROGRESS_VELOCITY, LIMIT_PROGRESS_VELOCITY);
skyyoungsik 1:9530746906b6 558 *OUTPUT_WANT_PITCH = -(err_Vel[0] * (GUI.gain_dx100[4] / 100.0));
skyyoungsik 1:9530746906b6 559 *OUTPUT_WANT_ROLL = +(err_Vel[1] * (GUI.gain_dx100[5] / 100.0));
skyyoungsik 1:9530746906b6 560
skyyoungsik 1:9530746906b6 561 /* ******** VEL PID - I **************** */
skyyoungsik 1:9530746906b6 562 PLANE_VEL_INTEG[0] -= (err_Vel[0]) * (GUI.gain_ix100[4] / 100.0);
skyyoungsik 1:9530746906b6 563 PLANE_VEL_INTEG[1] += (err_Vel[1]) * (GUI.gain_ix100[5] / 100.0);
skyyoungsik 1:9530746906b6 564 PLANE_VEL_INTEG[0] = MIN_MAX(PLANE_VEL_INTEG[0], -LIMIT_PLANE_VEL_INTEG, LIMIT_PLANE_VEL_INTEG);
skyyoungsik 1:9530746906b6 565 PLANE_VEL_INTEG[1] = MIN_MAX(PLANE_VEL_INTEG[1], -LIMIT_PLANE_VEL_INTEG, LIMIT_PLANE_VEL_INTEG);
skyyoungsik 1:9530746906b6 566 *OUTPUT_WANT_PITCH += PLANE_VEL_INTEG[0];
skyyoungsik 1:9530746906b6 567 *OUTPUT_WANT_ROLL += PLANE_VEL_INTEG[1];
skyyoungsik 1:9530746906b6 568
skyyoungsik 0:3473b92e991e 569 }
skyyoungsik 1:9530746906b6 570
skyyoungsik 1:9530746906b6 571 double VERTICAL_VEL_INTEG;
skyyoungsik 1:9530746906b6 572 void VERTICAL_VEL_CONTROL(double IN_VEL_Z, double *OUTPUT_THROTTLE_VALUE) {
skyyoungsik 1:9530746906b6 573 double err_Vel_Z;
skyyoungsik 1:9530746906b6 574 err_Vel_Z = IN_VEL_Z - SPATIAL.Vz;
skyyoungsik 1:9530746906b6 575 if (err_Vel_Z >= 0) err_Vel_Z = MIN_MAX(err_Vel_Z, -LIMIT_UP_VELOCITY, LIMIT_UP_VELOCITY);
skyyoungsik 1:9530746906b6 576 else err_Vel_Z = MIN_MAX(err_Vel_Z, -LIMIT_DOWN_VELOCITY, LIMIT_DOWN_VELOCITY);
skyyoungsik 1:9530746906b6 577 *OUTPUT_THROTTLE_VALUE = (err_Vel_Z * 10) * (GUI.gain_dx100[6] / 100.0);
skyyoungsik 1:9530746906b6 578
skyyoungsik 1:9530746906b6 579 VERTICAL_VEL_INTEG += (err_Vel_Z) * (GUI.gain_ix100[6] / 100.0);
skyyoungsik 1:9530746906b6 580 VERTICAL_VEL_INTEG = MIN_MAX(VERTICAL_VEL_INTEG, -LIMIT_VERTICAL_VEL_INTEG, LIMIT_VERTICAL_VEL_INTEG);
skyyoungsik 1:9530746906b6 581 *OUTPUT_THROTTLE_VALUE += VERTICAL_VEL_INTEG;
skyyoungsik 1:9530746906b6 582
skyyoungsik 1:9530746906b6 583 *OUTPUT_THROTTLE_VALUE = MIN_MAX(*OUTPUT_THROTTLE_VALUE, -THROTTLE_CONTROL_LIMIT, THROTTLE_CONTROL_LIMIT);
skyyoungsik 1:9530746906b6 584
skyyoungsik 1:9530746906b6 585 *OUTPUT_THROTTLE_VALUE += THROTTLE_BASE;
skyyoungsik 1:9530746906b6 586
skyyoungsik 1:9530746906b6 587 *OUTPUT_THROTTLE_VALUE = MIN_MAX( *OUTPUT_THROTTLE_VALUE, THROTTLE_MIN, THROTTLE_MAX );
skyyoungsik 1:9530746906b6 588
skyyoungsik 1:9530746906b6 589 }
skyyoungsik 1:9530746906b6 590
skyyoungsik 1:9530746906b6 591 void VEL_INTEG_RESET() {
skyyoungsik 1:9530746906b6 592 PLANE_VEL_INTEG[0] = 0; PLANE_VEL_INTEG[1] = 0; VERTICAL_VEL_INTEG = 0;
skyyoungsik 1:9530746906b6 593 }
skyyoungsik 1:9530746906b6 594
skyyoungsik 1:9530746906b6 595 float DEG_TO_METER(float lat1, float lon1, float lat2, float lon2) {
skyyoungsik 1:9530746906b6 596 float R = 6378.137; // Radius of earth in KM
skyyoungsik 1:9530746906b6 597 float dLat = lat2 * M_PI / 180.0 - lat1 * M_PI / 180.0;
skyyoungsik 1:9530746906b6 598 float dLon = lon2 * M_PI / 180.0 - lon1 * M_PI / 180.0;
skyyoungsik 1:9530746906b6 599 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);
skyyoungsik 1:9530746906b6 600 float c = 2 * atan2(sqrt(a), sqrt(1 - a));
skyyoungsik 1:9530746906b6 601 float d = R * c;
skyyoungsik 1:9530746906b6 602 return d * 1000; // meters
skyyoungsik 1:9530746906b6 603 }