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
main.cpp@0:a1ad0eb8b619, 2018-04-16 (annotated)
- Committer:
- skyyoungsik
- Date:
- Mon Apr 16 07:16:00 2018 +0000
- Revision:
- 0:a1ad0eb8b619
zg
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| skyyoungsik | 0:a1ad0eb8b619 | 1 | |
| skyyoungsik | 0:a1ad0eb8b619 | 2 | ////// 0SAS_FCC_V6 : Barometer 부착 |
| skyyoungsik | 0:a1ad0eb8b619 | 3 | ////// 0SAS_FCC_V7 : Barometer 제거 / External UART Output 추가 |
| skyyoungsik | 0:a1ad0eb8b619 | 4 | ////// 0SAS_FCC_V8 : PWM 최대 최소 추가, 초기 가속도 측정 wait 시간 늘리기 |
| skyyoungsik | 0:a1ad0eb8b619 | 5 | |
| skyyoungsik | 0:a1ad0eb8b619 | 6 | #include "mbed.h" |
| skyyoungsik | 0:a1ad0eb8b619 | 7 | #include "MPU6050.h" |
| skyyoungsik | 0:a1ad0eb8b619 | 8 | #include <math.h> |
| skyyoungsik | 0:a1ad0eb8b619 | 9 | #include "ROBOFRIEN_RCV.h" |
| skyyoungsik | 0:a1ad0eb8b619 | 10 | #include "ROBOFRIEN_GUI.h" |
| skyyoungsik | 0:a1ad0eb8b619 | 11 | #include "ROBOFRIEN_LED.h" |
| skyyoungsik | 0:a1ad0eb8b619 | 12 | #include "ROBOFRIEN_EXT_UART.h" |
| skyyoungsik | 0:a1ad0eb8b619 | 13 | |
| skyyoungsik | 0:a1ad0eb8b619 | 14 | //#define yaw_rate_control |
| skyyoungsik | 0:a1ad0eb8b619 | 15 | |
| skyyoungsik | 0:a1ad0eb8b619 | 16 | #define pwm_min 0.439 |
| skyyoungsik | 0:a1ad0eb8b619 | 17 | #define pwm_max 0.927 // pwm_min = 0.9ms, pwm_max = 1.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 18 | |
| skyyoungsik | 0:a1ad0eb8b619 | 19 | #ifdef yaw_rate_control |
| skyyoungsik | 0:a1ad0eb8b619 | 20 | #else |
| skyyoungsik | 0:a1ad0eb8b619 | 21 | #include "HMC5883L.h" |
| skyyoungsik | 0:a1ad0eb8b619 | 22 | HMC5883L hmc5883l(p9, p10); |
| skyyoungsik | 0:a1ad0eb8b619 | 23 | #endif |
| skyyoungsik | 0:a1ad0eb8b619 | 24 | |
| skyyoungsik | 0:a1ad0eb8b619 | 25 | ROBOFRIEN_EXT_UART EXT_UART; |
| skyyoungsik | 0:a1ad0eb8b619 | 26 | ROBOFRIEN_GUI GUI; |
| skyyoungsik | 0:a1ad0eb8b619 | 27 | ROBOFRIEN_LED LED; |
| skyyoungsik | 0:a1ad0eb8b619 | 28 | ROBOFRIEN_RCV RCV1(p5); |
| skyyoungsik | 0:a1ad0eb8b619 | 29 | ROBOFRIEN_RCV RCV2(p6); |
| skyyoungsik | 0:a1ad0eb8b619 | 30 | ROBOFRIEN_RCV RCV3(p7); |
| skyyoungsik | 0:a1ad0eb8b619 | 31 | ROBOFRIEN_RCV RCV4(p8); |
| skyyoungsik | 0:a1ad0eb8b619 | 32 | ROBOFRIEN_RCV RCV5(p11); |
| skyyoungsik | 0:a1ad0eb8b619 | 33 | ROBOFRIEN_RCV RCV6(p12); |
| skyyoungsik | 0:a1ad0eb8b619 | 34 | MPU6050 mpu6050; |
| skyyoungsik | 0:a1ad0eb8b619 | 35 | |
| skyyoungsik | 0:a1ad0eb8b619 | 36 | |
| skyyoungsik | 0:a1ad0eb8b619 | 37 | PwmOut PWM1(p26); |
| skyyoungsik | 0:a1ad0eb8b619 | 38 | PwmOut PWM2(p25); |
| skyyoungsik | 0:a1ad0eb8b619 | 39 | PwmOut PWM3(p24); |
| skyyoungsik | 0:a1ad0eb8b619 | 40 | PwmOut PWM4(p23); |
| skyyoungsik | 0:a1ad0eb8b619 | 41 | PwmOut PWM5(p22); |
| skyyoungsik | 0:a1ad0eb8b619 | 42 | PwmOut PWM6(p21); |
| skyyoungsik | 0:a1ad0eb8b619 | 43 | Ticker timer_500hz; |
| skyyoungsik | 0:a1ad0eb8b619 | 44 | |
| skyyoungsik | 0:a1ad0eb8b619 | 45 | Timer MainTimer; |
| skyyoungsik | 0:a1ad0eb8b619 | 46 | DigitalOut led2(LED2); |
| skyyoungsik | 0:a1ad0eb8b619 | 47 | DigitalOut led3(LED3); |
| skyyoungsik | 0:a1ad0eb8b619 | 48 | |
| skyyoungsik | 0:a1ad0eb8b619 | 49 | void compFilter(); |
| skyyoungsik | 0:a1ad0eb8b619 | 50 | void rc_cap_calibrate(); |
| skyyoungsik | 0:a1ad0eb8b619 | 51 | /// Setting data from Data Modem //// |
| skyyoungsik | 0:a1ad0eb8b619 | 52 | int16_t limit_integral_rate_roll,limit_integral_rate_pitch,limit_integral_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 53 | |
| skyyoungsik | 0:a1ad0eb8b619 | 54 | /////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 55 | float x, y, z, heading; |
| skyyoungsik | 0:a1ad0eb8b619 | 56 | 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; |
| skyyoungsik | 0:a1ad0eb8b619 | 57 | float pitchAngle = 0,rollAngle = 0,yawAngle = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 58 | int16_t gyro_data[3]; |
| skyyoungsik | 0:a1ad0eb8b619 | 59 | float roll_rate,pitch_rate,yaw_rate; |
| skyyoungsik | 0:a1ad0eb8b619 | 60 | float want_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 61 | void get_ahrs_data(); |
| skyyoungsik | 0:a1ad0eb8b619 | 62 | float get_compass_heading(); |
| skyyoungsik | 0:a1ad0eb8b619 | 63 | float complementary_roll,complementary_pitch,complementary_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 64 | float raw_mag_x, raw_mag_y, raw_mag_z; |
| skyyoungsik | 0:a1ad0eb8b619 | 65 | void compass_init(){ |
| skyyoungsik | 0:a1ad0eb8b619 | 66 | #ifdef yaw_rate_control |
| skyyoungsik | 0:a1ad0eb8b619 | 67 | |
| skyyoungsik | 0:a1ad0eb8b619 | 68 | #else |
| skyyoungsik | 0:a1ad0eb8b619 | 69 | raw_mag_x = hmc5883l.getMx(); |
| skyyoungsik | 0:a1ad0eb8b619 | 70 | raw_mag_y = -hmc5883l.getMy(); |
| skyyoungsik | 0:a1ad0eb8b619 | 71 | raw_mag_z = -hmc5883l.getMz(); |
| skyyoungsik | 0:a1ad0eb8b619 | 72 | want_yaw = get_compass_heading(); |
| skyyoungsik | 0:a1ad0eb8b619 | 73 | complementary_yaw = want_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 74 | #endif |
| skyyoungsik | 0:a1ad0eb8b619 | 75 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 76 | void ahrs_init(){ |
| skyyoungsik | 0:a1ad0eb8b619 | 77 | mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading |
| skyyoungsik | 0:a1ad0eb8b619 | 78 | mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers |
| skyyoungsik | 0:a1ad0eb8b619 | 79 | mpu6050.init(); // Initialize the sensor |
| skyyoungsik | 0:a1ad0eb8b619 | 80 | // timer_500hz.attach(&compFilter, 0.002); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) |
| skyyoungsik | 0:a1ad0eb8b619 | 81 | wait(0.5); |
| skyyoungsik | 0:a1ad0eb8b619 | 82 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 83 | |
| skyyoungsik | 0:a1ad0eb8b619 | 84 | /* This function is created to avoid address error that caused from Ticker.attach func */ |
| skyyoungsik | 0:a1ad0eb8b619 | 85 | void SAS_Algorithm(); |
| skyyoungsik | 0:a1ad0eb8b619 | 86 | int gap_time,current_ahrs_time,ex_ahrs_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 87 | bool ahrs_rcv_bool; |
| skyyoungsik | 0:a1ad0eb8b619 | 88 | void compFilter() { |
| skyyoungsik | 0:a1ad0eb8b619 | 89 | mpu6050.readAccelData(accelData); |
| skyyoungsik | 0:a1ad0eb8b619 | 90 | mpu6050.readGyroData(gyroData); |
| skyyoungsik | 0:a1ad0eb8b619 | 91 | #ifdef yaw_rate_control |
| skyyoungsik | 0:a1ad0eb8b619 | 92 | |
| skyyoungsik | 0:a1ad0eb8b619 | 93 | #else |
| skyyoungsik | 0:a1ad0eb8b619 | 94 | raw_mag_x = hmc5883l.getMx(); |
| skyyoungsik | 0:a1ad0eb8b619 | 95 | raw_mag_y = -hmc5883l.getMy(); |
| skyyoungsik | 0:a1ad0eb8b619 | 96 | raw_mag_z = -hmc5883l.getMz(); |
| skyyoungsik | 0:a1ad0eb8b619 | 97 | #endif |
| skyyoungsik | 0:a1ad0eb8b619 | 98 | ex_ahrs_time = current_ahrs_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 99 | current_ahrs_time = MainTimer.read_us(); |
| skyyoungsik | 0:a1ad0eb8b619 | 100 | gap_time = current_ahrs_time - ex_ahrs_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 101 | get_ahrs_data(); |
| skyyoungsik | 0:a1ad0eb8b619 | 102 | ahrs_rcv_bool = true; |
| skyyoungsik | 0:a1ad0eb8b619 | 103 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 104 | float get_compass_heading(){ |
| skyyoungsik | 0:a1ad0eb8b619 | 105 | float tilt_heading,no_tilt_heading; |
| skyyoungsik | 0:a1ad0eb8b619 | 106 | x = ( raw_mag_x - GUI.mag_x_avg )*0.92; |
| skyyoungsik | 0:a1ad0eb8b619 | 107 | y = ( raw_mag_y - GUI.mag_y_avg )*0.92; |
| skyyoungsik | 0:a1ad0eb8b619 | 108 | z = ( raw_mag_z - GUI.mag_z_avg )*0.92; |
| skyyoungsik | 0:a1ad0eb8b619 | 109 | |
| skyyoungsik | 0:a1ad0eb8b619 | 110 | //// TILT COMPENSATION ///// |
| skyyoungsik | 0:a1ad0eb8b619 | 111 | float cosRoll = cos(rollAngle*PI/180); |
| skyyoungsik | 0:a1ad0eb8b619 | 112 | float sinRoll = sin(rollAngle*PI/180); |
| skyyoungsik | 0:a1ad0eb8b619 | 113 | float cosPitch = cos(pitchAngle*PI/180); |
| skyyoungsik | 0:a1ad0eb8b619 | 114 | float sinPitch = sin(pitchAngle*PI/180); |
| skyyoungsik | 0:a1ad0eb8b619 | 115 | |
| skyyoungsik | 0:a1ad0eb8b619 | 116 | float Xh = x * cosRoll + y * sinPitch * sinRoll - z * cosPitch * sinRoll; |
| skyyoungsik | 0:a1ad0eb8b619 | 117 | float Yh = ( y * cosPitch + z * sinRoll ); |
| skyyoungsik | 0:a1ad0eb8b619 | 118 | // float heading = atan2(y,x); |
| skyyoungsik | 0:a1ad0eb8b619 | 119 | |
| skyyoungsik | 0:a1ad0eb8b619 | 120 | // heading = atan2(Yh, Xh) - (PI/2) + declination_angle*PI/180; |
| skyyoungsik | 0:a1ad0eb8b619 | 121 | // heading = atan2(Yh, Xh) + declination_angle*PI/180; |
| skyyoungsik | 0:a1ad0eb8b619 | 122 | // heading = -atan2(y,x); |
| skyyoungsik | 0:a1ad0eb8b619 | 123 | // heading = atan2(Yh,Xh); |
| skyyoungsik | 0:a1ad0eb8b619 | 124 | // heading = atan2(y,x) + declination_angle * PI/180; |
| skyyoungsik | 0:a1ad0eb8b619 | 125 | tilt_heading = atan2(Yh,Xh) - PI / 2 + GUI.declination_angle * PI/180; |
| skyyoungsik | 0:a1ad0eb8b619 | 126 | no_tilt_heading = (atan2(y,x) - PI / 2 + GUI.declination_angle * PI/180); |
| skyyoungsik | 0:a1ad0eb8b619 | 127 | |
| skyyoungsik | 0:a1ad0eb8b619 | 128 | if(tilt_heading < 0) |
| skyyoungsik | 0:a1ad0eb8b619 | 129 | tilt_heading += 2*PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 130 | if(tilt_heading > 2*PI) |
| skyyoungsik | 0:a1ad0eb8b619 | 131 | tilt_heading -= 2*PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 132 | |
| skyyoungsik | 0:a1ad0eb8b619 | 133 | tilt_heading = tilt_heading * 180 / PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 134 | |
| skyyoungsik | 0:a1ad0eb8b619 | 135 | if(no_tilt_heading < 0) |
| skyyoungsik | 0:a1ad0eb8b619 | 136 | no_tilt_heading += 2*PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 137 | if(no_tilt_heading > 2*PI) |
| skyyoungsik | 0:a1ad0eb8b619 | 138 | no_tilt_heading -= 2*PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 139 | |
| skyyoungsik | 0:a1ad0eb8b619 | 140 | no_tilt_heading = no_tilt_heading * 180 / PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 141 | |
| skyyoungsik | 0:a1ad0eb8b619 | 142 | return no_tilt_heading; |
| skyyoungsik | 0:a1ad0eb8b619 | 143 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 144 | void magnetic_offset_reset(){ |
| skyyoungsik | 0:a1ad0eb8b619 | 145 | max_m_x = raw_mag_x; |
| skyyoungsik | 0:a1ad0eb8b619 | 146 | min_m_x = raw_mag_x; |
| skyyoungsik | 0:a1ad0eb8b619 | 147 | max_m_y = raw_mag_y; |
| skyyoungsik | 0:a1ad0eb8b619 | 148 | min_m_y = raw_mag_y; |
| skyyoungsik | 0:a1ad0eb8b619 | 149 | max_m_z = raw_mag_z; |
| skyyoungsik | 0:a1ad0eb8b619 | 150 | min_m_z = raw_mag_z; |
| skyyoungsik | 0:a1ad0eb8b619 | 151 | GUI.mag_x_avg = (max_m_x + min_m_x)/2.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 152 | GUI.mag_y_avg = (max_m_y + min_m_y)/2.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 153 | GUI.mag_z_avg = (max_m_z + min_m_z)/2.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 154 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 155 | void magnetic_calibration(){ |
| skyyoungsik | 0:a1ad0eb8b619 | 156 | /// compass min / max check // |
| skyyoungsik | 0:a1ad0eb8b619 | 157 | if( raw_mag_x > max_m_x) max_m_x = raw_mag_x; |
| skyyoungsik | 0:a1ad0eb8b619 | 158 | if( raw_mag_x < min_m_x) min_m_x = raw_mag_x; |
| skyyoungsik | 0:a1ad0eb8b619 | 159 | if( raw_mag_y > max_m_y) max_m_y = raw_mag_y; |
| skyyoungsik | 0:a1ad0eb8b619 | 160 | if( raw_mag_y < min_m_y) min_m_y = raw_mag_y; |
| skyyoungsik | 0:a1ad0eb8b619 | 161 | if( raw_mag_z > max_m_z) max_m_z = raw_mag_z; |
| skyyoungsik | 0:a1ad0eb8b619 | 162 | if( raw_mag_z < min_m_z) min_m_z = raw_mag_z; |
| skyyoungsik | 0:a1ad0eb8b619 | 163 | GUI.mag_x_avg = (max_m_x + min_m_x)/2.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 164 | GUI.mag_y_avg = (max_m_y + min_m_y)/2.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 165 | GUI.mag_z_avg = (max_m_z + min_m_z)/2.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 166 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 167 | float ahrs_lpf_alpha; |
| skyyoungsik | 0:a1ad0eb8b619 | 168 | void get_ahrs_data(){ |
| skyyoungsik | 0:a1ad0eb8b619 | 169 | // mpu6050.complementaryFilter(&rollAngle, &pitchAngle); |
| skyyoungsik | 0:a1ad0eb8b619 | 170 | /* Get actual acc value */ |
| skyyoungsik | 0:a1ad0eb8b619 | 171 | mpu6050.getAres(); |
| skyyoungsik | 0:a1ad0eb8b619 | 172 | ax = accelData[0]*aRes - GUI.cal_accel_bias[0]; |
| skyyoungsik | 0:a1ad0eb8b619 | 173 | ay = accelData[1]*aRes - GUI.cal_accel_bias[1]; |
| skyyoungsik | 0:a1ad0eb8b619 | 174 | az = accelData[2]*aRes - GUI.cal_accel_bias[2]; |
| skyyoungsik | 0:a1ad0eb8b619 | 175 | |
| skyyoungsik | 0:a1ad0eb8b619 | 176 | /* Get actual gyro value */ |
| skyyoungsik | 0:a1ad0eb8b619 | 177 | mpu6050.getGres(); |
| skyyoungsik | 0:a1ad0eb8b619 | 178 | roll_rate = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i] |
| skyyoungsik | 0:a1ad0eb8b619 | 179 | pitch_rate = -gyroData[1]*gRes; // - gyroBias[1]; |
| skyyoungsik | 0:a1ad0eb8b619 | 180 | yaw_rate = -gyroData[2]*gRes; // - gyroBias[2]; |
| skyyoungsik | 0:a1ad0eb8b619 | 181 | |
| skyyoungsik | 0:a1ad0eb8b619 | 182 | /* Integrate the gyro data(deg/s) over time to get angle */ |
| skyyoungsik | 0:a1ad0eb8b619 | 183 | complementary_roll += roll_rate * (float)gap_time/1000000.0; // Angle around the X-axis |
| skyyoungsik | 0:a1ad0eb8b619 | 184 | complementary_pitch += pitch_rate * (float)gap_time/1000000.0; // Angle around the Y-axis |
| skyyoungsik | 0:a1ad0eb8b619 | 185 | complementary_yaw += yaw_rate * (float)gap_time/1000000.0; // Angle around the Z-axis |
| skyyoungsik | 0:a1ad0eb8b619 | 186 | |
| skyyoungsik | 0:a1ad0eb8b619 | 187 | float pitchAcc, rollAcc; |
| skyyoungsik | 0:a1ad0eb8b619 | 188 | // rollAcc = atan2f(accelData[1], accelData[2])*180/PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 189 | // pitchAcc = atan2f(accelData[0], accelData[2])*180/PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 190 | rollAcc = atan2f(ay, az)*180/PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 191 | pitchAcc = atan2f(ax, az)*180/PI; |
| skyyoungsik | 0:a1ad0eb8b619 | 192 | /* |
| skyyoungsik | 0:a1ad0eb8b619 | 193 | float forceMagnitudeApprox = sqrt( (ax*ax) + (ay*ay) + (az*az) ); |
| skyyoungsik | 0:a1ad0eb8b619 | 194 | float accelerometer_factor = (2 - forceMagnitudeApprox); // At 2G accelerometer_factor = 0 // Range = 0.0 ~ 1.0 |
| skyyoungsik | 0:a1ad0eb8b619 | 195 | if(accelerometer_factor >= 1) accelerometer_factor = 1; |
| skyyoungsik | 0:a1ad0eb8b619 | 196 | else if(accelerometer_factor <= 0) accelerometer_factor = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 197 | rollAngle = rollAngle * ( 1 - accelerometer_factor / 50.0 ) + rollAcc * (accelerometer_factor / 50.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 198 | pitchAngle = pitchAngle * ( 1 - accelerometer_factor / 50.0 ) + pitchAcc * (accelerometer_factor / 50.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 199 | */ |
| skyyoungsik | 0:a1ad0eb8b619 | 200 | complementary_roll = complementary_roll * ahrs_lpf_alpha + rollAcc * (1-ahrs_lpf_alpha); |
| skyyoungsik | 0:a1ad0eb8b619 | 201 | complementary_pitch = complementary_pitch * ahrs_lpf_alpha + pitchAcc * (1-ahrs_lpf_alpha); |
| skyyoungsik | 0:a1ad0eb8b619 | 202 | |
| skyyoungsik | 0:a1ad0eb8b619 | 203 | rollAngle = complementary_roll;// - GUI.calibrate_gap_roll; |
| skyyoungsik | 0:a1ad0eb8b619 | 204 | pitchAngle = complementary_pitch;// - GUI.calibrate_gap_pitch; |
| skyyoungsik | 0:a1ad0eb8b619 | 205 | |
| skyyoungsik | 0:a1ad0eb8b619 | 206 | #ifdef yaw_rate_control |
| skyyoungsik | 0:a1ad0eb8b619 | 207 | |
| skyyoungsik | 0:a1ad0eb8b619 | 208 | #else |
| skyyoungsik | 0:a1ad0eb8b619 | 209 | /// CAL COMPASS YAW //// |
| skyyoungsik | 0:a1ad0eb8b619 | 210 | float data_yaw_to_180; |
| skyyoungsik | 0:a1ad0eb8b619 | 211 | data_yaw_to_180 = 180 - complementary_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 212 | |
| skyyoungsik | 0:a1ad0eb8b619 | 213 | complementary_yaw = 180; |
| skyyoungsik | 0:a1ad0eb8b619 | 214 | float tmp_compass_heading; |
| skyyoungsik | 0:a1ad0eb8b619 | 215 | tmp_compass_heading = get_compass_heading() + data_yaw_to_180; |
| skyyoungsik | 0:a1ad0eb8b619 | 216 | if (tmp_compass_heading > 360) tmp_compass_heading -= 360; |
| skyyoungsik | 0:a1ad0eb8b619 | 217 | if (tmp_compass_heading < 0) tmp_compass_heading += 360; |
| skyyoungsik | 0:a1ad0eb8b619 | 218 | complementary_yaw = complementary_yaw*0.999 + tmp_compass_heading*0.001; |
| skyyoungsik | 0:a1ad0eb8b619 | 219 | |
| skyyoungsik | 0:a1ad0eb8b619 | 220 | complementary_yaw -= data_yaw_to_180; |
| skyyoungsik | 0:a1ad0eb8b619 | 221 | #endif |
| skyyoungsik | 0:a1ad0eb8b619 | 222 | if (complementary_yaw > 360) complementary_yaw -= 360; |
| skyyoungsik | 0:a1ad0eb8b619 | 223 | if (complementary_yaw < 0) complementary_yaw += 360; |
| skyyoungsik | 0:a1ad0eb8b619 | 224 | |
| skyyoungsik | 0:a1ad0eb8b619 | 225 | yawAngle = complementary_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 226 | |
| skyyoungsik | 0:a1ad0eb8b619 | 227 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 228 | |
| skyyoungsik | 0:a1ad0eb8b619 | 229 | |
| skyyoungsik | 0:a1ad0eb8b619 | 230 | //////////////// SAS /////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 231 | float integral_rate_roll,integral_rate_pitch,integral_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 232 | int32_t error_rate_roll = 0, error_rate_pitch = 0, error_rate_yaw = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 233 | int32_t derivate_lpf_roll, derivate_lpf_pitch, derivate_lpf_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 234 | void SAS_Algorithm(){ |
| skyyoungsik | 0:a1ad0eb8b619 | 235 | int16_t stick_roll, stick_pitch, stick_throttle, stick_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 236 | stick_roll = GUI.cap[0] * (GUI.limit_rollx100 / 100); // (-100 ~ 100) to (limit angle x 100) |
| skyyoungsik | 0:a1ad0eb8b619 | 237 | stick_pitch = GUI.cap[1] * (GUI.limit_pitchx100 / 100); // (-100 ~ 100) to (limit angle x 100) |
| skyyoungsik | 0:a1ad0eb8b619 | 238 | stick_throttle = (GUI.cap[2] + 100) * 5; // (-100 ~ 100) to (0 ~ 1000) |
| skyyoungsik | 0:a1ad0eb8b619 | 239 | stick_yaw = GUI.cap[3] * 45; // (-100 ~ 100) to (-4500 ~ 4500) |
| skyyoungsik | 0:a1ad0eb8b619 | 240 | |
| skyyoungsik | 0:a1ad0eb8b619 | 241 | // WANT YAW - generate // |
| skyyoungsik | 0:a1ad0eb8b619 | 242 | if( ( stick_yaw < -100 ) | ( stick_yaw > 100 ) ){ |
| skyyoungsik | 0:a1ad0eb8b619 | 243 | want_yaw += (float)stick_yaw / 10000.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 244 | if(want_yaw < 0) want_yaw += 360; |
| skyyoungsik | 0:a1ad0eb8b619 | 245 | if(want_yaw > 360) want_yaw -= 360; |
| skyyoungsik | 0:a1ad0eb8b619 | 246 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 247 | int16_t error_roll,error_pitch,error_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 248 | error_roll = stick_roll - rollAngle*100; |
| skyyoungsik | 0:a1ad0eb8b619 | 249 | error_pitch = stick_pitch - pitchAngle*100; |
| skyyoungsik | 0:a1ad0eb8b619 | 250 | |
| skyyoungsik | 0:a1ad0eb8b619 | 251 | #ifdef yaw_rate_control |
| skyyoungsik | 0:a1ad0eb8b619 | 252 | error_yaw = stick_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 253 | #else |
| skyyoungsik | 0:a1ad0eb8b619 | 254 | if(want_yaw <= 180){ |
| skyyoungsik | 0:a1ad0eb8b619 | 255 | if( yawAngle <= ( 180 + want_yaw ) ) error_yaw = (want_yaw - yawAngle)*100; |
| skyyoungsik | 0:a1ad0eb8b619 | 256 | else error_yaw = ((want_yaw + 360) - yawAngle )*100; |
| skyyoungsik | 0:a1ad0eb8b619 | 257 | }else{ |
| skyyoungsik | 0:a1ad0eb8b619 | 258 | if( yawAngle >= ( want_yaw - 180 ) ) error_yaw = (want_yaw - yawAngle)*100; |
| skyyoungsik | 0:a1ad0eb8b619 | 259 | else error_yaw = (want_yaw - (yawAngle + 360))*100; |
| skyyoungsik | 0:a1ad0eb8b619 | 260 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 261 | #endif |
| skyyoungsik | 0:a1ad0eb8b619 | 262 | if(error_roll >= 4500) error_roll = 4500; |
| skyyoungsik | 0:a1ad0eb8b619 | 263 | else if(error_roll <= -4500) error_roll = -4500; |
| skyyoungsik | 0:a1ad0eb8b619 | 264 | if(error_pitch >= 4500) error_pitch = 4500; |
| skyyoungsik | 0:a1ad0eb8b619 | 265 | else if(error_pitch <= -4500) error_pitch = -4500; |
| skyyoungsik | 0:a1ad0eb8b619 | 266 | if(error_yaw >= 3000) error_yaw = 3000; |
| skyyoungsik | 0:a1ad0eb8b619 | 267 | else if(error_yaw <= -3000) error_yaw = -3000; |
| skyyoungsik | 0:a1ad0eb8b619 | 268 | |
| skyyoungsik | 0:a1ad0eb8b619 | 269 | /// Convert [-4500 ~ 4500] -->> [-20250 ~ 202500 degx100/s] |
| skyyoungsik | 0:a1ad0eb8b619 | 270 | int16_t earth_want_rate_roll = 0, earth_want_rate_pitch = 0, earth_want_rate_yaw = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 271 | earth_want_rate_roll = (int32_t)error_roll * GUI.gain_px100[0] / 100.0; // about -100 ~ 100 |
| skyyoungsik | 0:a1ad0eb8b619 | 272 | earth_want_rate_pitch = (int32_t)error_pitch * GUI.gain_px100[2] / 100.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 273 | earth_want_rate_yaw = (int32_t)error_yaw * GUI.gain_px100[4] / 100.0; |
| skyyoungsik | 0:a1ad0eb8b619 | 274 | |
| skyyoungsik | 0:a1ad0eb8b619 | 275 | int16_t body_want_rate_roll = 0, body_want_rate_pitch = 0, body_want_rate_yaw = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 276 | float sin_roll, sin_pitch, cos_roll, cos_pitch; |
| skyyoungsik | 0:a1ad0eb8b619 | 277 | sin_roll = sin((rollAngle)*PI / 180); |
| skyyoungsik | 0:a1ad0eb8b619 | 278 | sin_pitch = sin((pitchAngle)*PI / 180); |
| skyyoungsik | 0:a1ad0eb8b619 | 279 | cos_roll = cos((rollAngle)*PI / 180); |
| skyyoungsik | 0:a1ad0eb8b619 | 280 | cos_pitch = cos((pitchAngle)*PI / 180); |
| skyyoungsik | 0:a1ad0eb8b619 | 281 | body_want_rate_roll = earth_want_rate_roll - sin_pitch * earth_want_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 282 | body_want_rate_pitch = cos_roll * earth_want_rate_pitch + sin_roll * cos_pitch * earth_want_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 283 | body_want_rate_yaw = -sin_roll * earth_want_rate_pitch + cos_pitch * cos_roll * earth_want_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 284 | |
| skyyoungsik | 0:a1ad0eb8b619 | 285 | int32_t ex_error_rate_roll = 0, ex_error_rate_pitch = 0, ex_error_rate_yaw = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 286 | ex_error_rate_roll = error_rate_roll; |
| skyyoungsik | 0:a1ad0eb8b619 | 287 | ex_error_rate_pitch = error_rate_pitch; |
| skyyoungsik | 0:a1ad0eb8b619 | 288 | ex_error_rate_yaw = error_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 289 | error_rate_roll = ((int32_t)body_want_rate_roll - roll_rate * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 290 | error_rate_pitch = ((int32_t)body_want_rate_pitch - pitch_rate * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 291 | error_rate_yaw = ((int32_t)body_want_rate_yaw - yaw_rate * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 292 | // error_rate_roll = ((int32_t)earth_want_rate_roll - roll_rate * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 293 | // error_rate_pitch = ((int32_t)earth_want_rate_pitch - pitch_rate * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 294 | // error_rate_yaw = ((int32_t)earth_want_rate_yaw - yaw_rate * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 295 | |
| skyyoungsik | 0:a1ad0eb8b619 | 296 | //// RATE PID //// |
| skyyoungsik | 0:a1ad0eb8b619 | 297 | //// ---- P Gain ---- //// |
| skyyoungsik | 0:a1ad0eb8b619 | 298 | int16_t stack_roll = 0, stack_pitch = 0, stack_yaw = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 299 | stack_roll = ((float)error_rate_roll * (float)GUI.gain_px100[1] / 100.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 300 | stack_pitch = ((float)error_rate_pitch * (float)GUI.gain_px100[3] / 100.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 301 | stack_yaw = ((float)error_rate_yaw * (float)GUI.gain_px100[5] / 100.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 302 | |
| skyyoungsik | 0:a1ad0eb8b619 | 303 | //// ---- I Gain ---- //// |
| skyyoungsik | 0:a1ad0eb8b619 | 304 | integral_rate_roll += ((float)error_rate_roll * ((float)gap_time/1000000.0) * (float)GUI.gain_ix100[1] / 100.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 305 | integral_rate_pitch += ((float)error_rate_pitch * ((float)gap_time/1000000.0) * (float)GUI.gain_ix100[3] / 100.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 306 | integral_rate_yaw += ((float)error_rate_yaw * ((float)gap_time/1000000.0) * (float)GUI.gain_ix100[5] / 100.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 307 | if(integral_rate_roll > limit_integral_rate_roll) integral_rate_roll = limit_integral_rate_roll; |
| skyyoungsik | 0:a1ad0eb8b619 | 308 | else if(integral_rate_roll < -limit_integral_rate_roll) integral_rate_roll = -limit_integral_rate_roll; |
| skyyoungsik | 0:a1ad0eb8b619 | 309 | if(integral_rate_pitch > limit_integral_rate_pitch) integral_rate_pitch = limit_integral_rate_pitch; |
| skyyoungsik | 0:a1ad0eb8b619 | 310 | else if(integral_rate_pitch < -limit_integral_rate_pitch) integral_rate_pitch = -limit_integral_rate_pitch; |
| skyyoungsik | 0:a1ad0eb8b619 | 311 | if(integral_rate_yaw > limit_integral_rate_yaw) integral_rate_yaw = limit_integral_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 312 | else if(integral_rate_yaw < -limit_integral_rate_yaw) integral_rate_yaw = -limit_integral_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 313 | stack_roll += integral_rate_roll; |
| skyyoungsik | 0:a1ad0eb8b619 | 314 | stack_pitch += integral_rate_pitch; |
| skyyoungsik | 0:a1ad0eb8b619 | 315 | stack_yaw += integral_rate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 316 | |
| skyyoungsik | 0:a1ad0eb8b619 | 317 | //// ------ D Gain ----- ///// |
| skyyoungsik | 0:a1ad0eb8b619 | 318 | float derivate_roll, derivate_pitch, derivate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 319 | derivate_roll = ((float)(error_rate_roll - ex_error_rate_roll))/((float)gap_time/1000000.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 320 | derivate_pitch = ((float)(error_rate_pitch - ex_error_rate_pitch))/((float)gap_time/1000000.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 321 | derivate_yaw = ((float)(error_rate_yaw - ex_error_rate_yaw))/((float)gap_time/1000000.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 322 | derivate_roll = (float)derivate_roll * ((float)GUI.gain_dx100[1] / 1000.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 323 | derivate_pitch = (float)derivate_pitch * ((float)GUI.gain_dx100[3] / 1000.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 324 | derivate_yaw = (float)derivate_yaw * ((float)GUI.gain_dx100[5] / 1000.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 325 | derivate_lpf_roll = derivate_lpf_roll * 0.9 + derivate_roll * 0.1; |
| skyyoungsik | 0:a1ad0eb8b619 | 326 | derivate_lpf_pitch = derivate_lpf_pitch * 0.9 + derivate_pitch * 0.1; |
| skyyoungsik | 0:a1ad0eb8b619 | 327 | derivate_lpf_yaw = derivate_lpf_yaw * 0.9 + derivate_yaw * 0.1; |
| skyyoungsik | 0:a1ad0eb8b619 | 328 | // derivate_lpf_roll = derivate_roll; |
| skyyoungsik | 0:a1ad0eb8b619 | 329 | // derivate_lpf_pitch = derivate_pitch; |
| skyyoungsik | 0:a1ad0eb8b619 | 330 | // derivate_lpf_yaw = derivate_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 331 | stack_roll += derivate_lpf_roll; |
| skyyoungsik | 0:a1ad0eb8b619 | 332 | stack_pitch += derivate_lpf_pitch; |
| skyyoungsik | 0:a1ad0eb8b619 | 333 | stack_yaw += derivate_lpf_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 334 | |
| skyyoungsik | 0:a1ad0eb8b619 | 335 | /// stack limit ///// |
| skyyoungsik | 0:a1ad0eb8b619 | 336 | if(stack_roll > 5000) stack_roll = 5000; |
| skyyoungsik | 0:a1ad0eb8b619 | 337 | else if(stack_roll < -5000) stack_roll = -5000; |
| skyyoungsik | 0:a1ad0eb8b619 | 338 | if(stack_pitch > 5000) stack_pitch = 5000; |
| skyyoungsik | 0:a1ad0eb8b619 | 339 | else if(stack_pitch < -5000) stack_pitch = -5000; |
| skyyoungsik | 0:a1ad0eb8b619 | 340 | if(stack_yaw > 5000) stack_yaw = 5000; |
| skyyoungsik | 0:a1ad0eb8b619 | 341 | else if(stack_yaw < -5000) stack_yaw = -5000; |
| skyyoungsik | 0:a1ad0eb8b619 | 342 | |
| skyyoungsik | 0:a1ad0eb8b619 | 343 | int16_t raw_esc_pwm[4]; |
| skyyoungsik | 0:a1ad0eb8b619 | 344 | raw_esc_pwm[0] = ((float)stick_throttle * 7.5) + (+ stack_roll + stack_pitch - stack_yaw); |
| skyyoungsik | 0:a1ad0eb8b619 | 345 | raw_esc_pwm[1] = ((float)stick_throttle * 7.5) + (- stack_roll + stack_pitch + stack_yaw); |
| skyyoungsik | 0:a1ad0eb8b619 | 346 | raw_esc_pwm[2] = ((float)stick_throttle * 7.5) + (- stack_roll - stack_pitch - stack_yaw); |
| skyyoungsik | 0:a1ad0eb8b619 | 347 | raw_esc_pwm[3] = ((float)stick_throttle * 7.5) + (+ stack_roll - stack_pitch + stack_yaw); |
| skyyoungsik | 0:a1ad0eb8b619 | 348 | |
| skyyoungsik | 0:a1ad0eb8b619 | 349 | for (int i = 0; i < 4; i++) { |
| skyyoungsik | 0:a1ad0eb8b619 | 350 | if (raw_esc_pwm[i] > 10000) raw_esc_pwm[i] = 10000; |
| skyyoungsik | 0:a1ad0eb8b619 | 351 | else if (raw_esc_pwm[i] < 0) raw_esc_pwm[i] = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 352 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 353 | uint16_t esc_pwm[4]; |
| skyyoungsik | 0:a1ad0eb8b619 | 354 | for (int i = 0; i < 4; i++){ |
| skyyoungsik | 0:a1ad0eb8b619 | 355 | 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;} |
| skyyoungsik | 0:a1ad0eb8b619 | 356 | else esc_pwm[i] = raw_esc_pwm[i] + ((float)GUI.motor_min[i]*10); |
| skyyoungsik | 0:a1ad0eb8b619 | 357 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 358 | |
| skyyoungsik | 0:a1ad0eb8b619 | 359 | float pwm_data[4]; |
| skyyoungsik | 0:a1ad0eb8b619 | 360 | if(GUI.DPN_Info == 0){ |
| skyyoungsik | 0:a1ad0eb8b619 | 361 | pwm_data[0] = (float)esc_pwm[0]/10000.0 * (pwm_max - pwm_min) + pwm_min; // 0.439 = 0.9ms , 0.927 = 1.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 362 | pwm_data[1] = (float)esc_pwm[1]/10000.0 * (pwm_max - pwm_min) + pwm_min; // 0.439 = 0.9ms , 0.927 = 1.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 363 | pwm_data[2] = (float)esc_pwm[2]/10000.0 * (pwm_max - pwm_min) + pwm_min; // 0.439 = 0.9ms , 0.927 = 1.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 364 | pwm_data[3] = (float)esc_pwm[3]/10000.0 * (pwm_max - pwm_min) + pwm_min; // 0.439 = 0.9ms , 0.927 = 1.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 365 | }else if(GUI.DPN_Info == 4){ |
| skyyoungsik | 0:a1ad0eb8b619 | 366 | pwm_data[0] = (float)GUI.pwm_info[0]/1000.0 * (pwm_max - pwm_min) + pwm_min; |
| skyyoungsik | 0:a1ad0eb8b619 | 367 | pwm_data[1] = (float)GUI.pwm_info[1]/1000.0 * (pwm_max - pwm_min) + pwm_min; |
| skyyoungsik | 0:a1ad0eb8b619 | 368 | pwm_data[2] = (float)GUI.pwm_info[2]/1000.0 * (pwm_max - pwm_min) + pwm_min; |
| skyyoungsik | 0:a1ad0eb8b619 | 369 | pwm_data[3] = (float)GUI.pwm_info[3]/1000.0 * (pwm_max - pwm_min) + pwm_min; |
| skyyoungsik | 0:a1ad0eb8b619 | 370 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 371 | // Convert [ 0 ~ 10000 ] to [ 0 ~ 200 ] // |
| skyyoungsik | 0:a1ad0eb8b619 | 372 | for (int i = 0; i < 4; i++){ |
| skyyoungsik | 0:a1ad0eb8b619 | 373 | if(pwm_data[i] > pwm_max) pwm_data[i] = pwm_max; |
| skyyoungsik | 0:a1ad0eb8b619 | 374 | else if(pwm_data[i] < pwm_min) pwm_data[i] = pwm_min; |
| skyyoungsik | 0:a1ad0eb8b619 | 375 | GUI.pwm[i] = (esc_pwm[i] / 50); |
| skyyoungsik | 0:a1ad0eb8b619 | 376 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 377 | PWM1 = pwm_data[0]; |
| skyyoungsik | 0:a1ad0eb8b619 | 378 | PWM2 = pwm_data[1]; |
| skyyoungsik | 0:a1ad0eb8b619 | 379 | PWM3 = pwm_data[2]; |
| skyyoungsik | 0:a1ad0eb8b619 | 380 | PWM4 = pwm_data[3]; |
| skyyoungsik | 0:a1ad0eb8b619 | 381 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 382 | |
| skyyoungsik | 0:a1ad0eb8b619 | 383 | |
| skyyoungsik | 0:a1ad0eb8b619 | 384 | float calibrated_cap[6]; |
| skyyoungsik | 0:a1ad0eb8b619 | 385 | void rc_cap_calibrate(){ |
| skyyoungsik | 0:a1ad0eb8b619 | 386 | float tmp_rcv_lpf_cap[6]; |
| skyyoungsik | 0:a1ad0eb8b619 | 387 | tmp_rcv_lpf_cap[0] = (RCV1.lpf_cap[0]-150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 388 | tmp_rcv_lpf_cap[1] = (RCV2.lpf_cap[1]-150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 389 | tmp_rcv_lpf_cap[2] = (RCV3.lpf_cap[2]-150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 390 | tmp_rcv_lpf_cap[3] = (RCV4.lpf_cap[3]-150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 391 | tmp_rcv_lpf_cap[4] = (RCV5.lpf_cap[4]-150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 392 | tmp_rcv_lpf_cap[5] = (RCV6.lpf_cap[5]-150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 393 | for(int i=0; i<6; i++){ |
| skyyoungsik | 0:a1ad0eb8b619 | 394 | if( tmp_rcv_lpf_cap[i] >= GUI.cap_neu[i] ) { |
| skyyoungsik | 0:a1ad0eb8b619 | 395 | calibrated_cap[i] = (float)(tmp_rcv_lpf_cap[i] - GUI.cap_neu[i]) * 100 / (GUI.cap_max[i] - GUI.cap_neu[i]); |
| skyyoungsik | 0:a1ad0eb8b619 | 396 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 397 | else{ |
| skyyoungsik | 0:a1ad0eb8b619 | 398 | calibrated_cap[i] = (float)(tmp_rcv_lpf_cap[i] - GUI.cap_neu[i]) * 100 / (GUI.cap_neu[i] - GUI.cap_min[i]); |
| skyyoungsik | 0:a1ad0eb8b619 | 399 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 400 | ///// MIN MAX //// |
| skyyoungsik | 0:a1ad0eb8b619 | 401 | if (calibrated_cap[i] >= 100) calibrated_cap[i] = 100; |
| skyyoungsik | 0:a1ad0eb8b619 | 402 | else if (calibrated_cap[i] <= -100) calibrated_cap[i] = -100; |
| skyyoungsik | 0:a1ad0eb8b619 | 403 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 404 | ////// SELECT Between EXT_UART and RC Receiver ///// |
| skyyoungsik | 0:a1ad0eb8b619 | 405 | GUI.cap[4] = calibrated_cap[4]; |
| skyyoungsik | 0:a1ad0eb8b619 | 406 | GUI.cap[5] = calibrated_cap[5]; |
| skyyoungsik | 0:a1ad0eb8b619 | 407 | for(int i=0; i<4; i++){ |
| skyyoungsik | 0:a1ad0eb8b619 | 408 | if(GUI.cap[4] > 50){ |
| skyyoungsik | 0:a1ad0eb8b619 | 409 | GUI.cap[i] = EXT_UART.uart_stick_cap[i]; |
| skyyoungsik | 0:a1ad0eb8b619 | 410 | }else{ |
| skyyoungsik | 0:a1ad0eb8b619 | 411 | GUI.cap[i] = calibrated_cap[i]; |
| skyyoungsik | 0:a1ad0eb8b619 | 412 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 413 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 414 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 415 | |
| skyyoungsik | 0:a1ad0eb8b619 | 416 | |
| skyyoungsik | 0:a1ad0eb8b619 | 417 | |
| skyyoungsik | 0:a1ad0eb8b619 | 418 | int cnt_1hz; |
| skyyoungsik | 0:a1ad0eb8b619 | 419 | int current_system_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 420 | int ex_millis_2ms,ex_millis_5ms,ex_millis_20ms,ex_millis_50ms,ex_millis_100ms,ex_millis_1000ms; |
| skyyoungsik | 0:a1ad0eb8b619 | 421 | int ex_millis_ahrs_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 422 | int cnt; |
| skyyoungsik | 0:a1ad0eb8b619 | 423 | uint8_t ex_Compass_Calibration_Mode; |
| skyyoungsik | 0:a1ad0eb8b619 | 424 | int main() { |
| skyyoungsik | 0:a1ad0eb8b619 | 425 | /// PWM //// |
| skyyoungsik | 0:a1ad0eb8b619 | 426 | PWM1 = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 427 | PWM2 = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 428 | PWM3 = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 429 | PWM4 = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 430 | PWM1.period(0.00205); // set PWM period to 2.05ms |
| skyyoungsik | 0:a1ad0eb8b619 | 431 | PWM2.period(0.00205); // set PWM period to 2.05ms |
| skyyoungsik | 0:a1ad0eb8b619 | 432 | PWM3.period(0.00205); // set PWM period to 2.05ms |
| skyyoungsik | 0:a1ad0eb8b619 | 433 | PWM4.period(0.00205); // set PWM period to 2.05ms |
| skyyoungsik | 0:a1ad0eb8b619 | 434 | PWM1 = pwm_min; // set duty cycle to 0.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 435 | PWM2 = pwm_min; // set duty cycle to 0.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 436 | PWM3 = pwm_min; // set duty cycle to 0.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 437 | PWM4 = pwm_min; // set duty cycle to 0.9ms |
| skyyoungsik | 0:a1ad0eb8b619 | 438 | |
| skyyoungsik | 0:a1ad0eb8b619 | 439 | MainTimer.start(); |
| skyyoungsik | 0:a1ad0eb8b619 | 440 | GUI.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 441 | LED.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 442 | RCV1.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 443 | // RCV2.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 444 | // RCV3.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 445 | // RCV4.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 446 | // RCV5.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 447 | // RCV6.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 448 | ahrs_init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 449 | wait(1); |
| skyyoungsik | 0:a1ad0eb8b619 | 450 | //// Data from Data Modem //// |
| skyyoungsik | 0:a1ad0eb8b619 | 451 | // 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; |
| skyyoungsik | 0:a1ad0eb8b619 | 452 | // 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; |
| skyyoungsik | 0:a1ad0eb8b619 | 453 | // 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; |
| skyyoungsik | 0:a1ad0eb8b619 | 454 | // limit_rollx100 = 4500,limit_pitchx100 = 4500; |
| skyyoungsik | 0:a1ad0eb8b619 | 455 | // gain_px100[0] = 450; gain_px100[1] = 15; gain_ix100[1] = 10; gain_dx100[1] = 4; |
| skyyoungsik | 0:a1ad0eb8b619 | 456 | // gain_px100[2] = 450; gain_px100[3] = 15; gain_ix100[3] = 10; gain_dx100[3] = 4; |
| skyyoungsik | 0:a1ad0eb8b619 | 457 | // gain_px100[4] = 450; gain_px100[5] = 60; gain_ix100[5] = 20; gain_dx100[5] = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 458 | limit_integral_rate_roll = 500; limit_integral_rate_pitch = 500; limit_integral_rate_yaw = 500; |
| skyyoungsik | 0:a1ad0eb8b619 | 459 | // init_motor_gap[0] = 49; |
| skyyoungsik | 0:a1ad0eb8b619 | 460 | // init_motor_gap[1] = 49; |
| skyyoungsik | 0:a1ad0eb8b619 | 461 | // init_motor_gap[2] = 49; |
| skyyoungsik | 0:a1ad0eb8b619 | 462 | // init_motor_gap[3] = 49; |
| skyyoungsik | 0:a1ad0eb8b619 | 463 | |
| skyyoungsik | 0:a1ad0eb8b619 | 464 | led2 = 1; |
| skyyoungsik | 0:a1ad0eb8b619 | 465 | led3 = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 466 | while(1){ |
| skyyoungsik | 0:a1ad0eb8b619 | 467 | wait(0.5); |
| skyyoungsik | 0:a1ad0eb8b619 | 468 | ahrs_init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 469 | wait(0.005); |
| skyyoungsik | 0:a1ad0eb8b619 | 470 | compFilter(); |
| skyyoungsik | 0:a1ad0eb8b619 | 471 | |
| skyyoungsik | 0:a1ad0eb8b619 | 472 | float gyro_limit = 0.3; |
| skyyoungsik | 0:a1ad0eb8b619 | 473 | 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;} |
| skyyoungsik | 0:a1ad0eb8b619 | 474 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 475 | led2 = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 476 | led3 = 1; |
| skyyoungsik | 0:a1ad0eb8b619 | 477 | ahrs_lpf_alpha = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 478 | current_system_time = MainTimer.read_us(); |
| skyyoungsik | 0:a1ad0eb8b619 | 479 | ex_millis_ahrs_time = current_system_time - 500; |
| skyyoungsik | 0:a1ad0eb8b619 | 480 | compFilter(); |
| skyyoungsik | 0:a1ad0eb8b619 | 481 | |
| skyyoungsik | 0:a1ad0eb8b619 | 482 | led2 = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 483 | led3 = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 484 | |
| skyyoungsik | 0:a1ad0eb8b619 | 485 | ahrs_lpf_alpha = 0.997; |
| skyyoungsik | 0:a1ad0eb8b619 | 486 | // 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 ){ |
| skyyoungsik | 0:a1ad0eb8b619 | 487 | while( ( RCV1.rc_state[0] & RCV2.rc_state[1] & RCV3.rc_state[2] & RCV4.rc_state[3] & RCV5.rc_state[4] ) == false ){ |
| skyyoungsik | 0:a1ad0eb8b619 | 488 | led3 = 1; wait(0.5); |
| skyyoungsik | 0:a1ad0eb8b619 | 489 | led3 = 0; wait(0.5); |
| skyyoungsik | 0:a1ad0eb8b619 | 490 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 491 | |
| skyyoungsik | 0:a1ad0eb8b619 | 492 | compass_init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 493 | |
| skyyoungsik | 0:a1ad0eb8b619 | 494 | LED.HeadlightPeriod = GUI.headlight_period; |
| skyyoungsik | 0:a1ad0eb8b619 | 495 | LED.HeadlightDutyrate = GUI.headlight_dutyrate; |
| skyyoungsik | 0:a1ad0eb8b619 | 496 | LED.SidelightPeriod = GUI.sidelight_period; |
| skyyoungsik | 0:a1ad0eb8b619 | 497 | LED.SidelightDutyrate = GUI.sidelight_dutyrate; |
| skyyoungsik | 0:a1ad0eb8b619 | 498 | |
| skyyoungsik | 0:a1ad0eb8b619 | 499 | EXT_UART.Init(); |
| skyyoungsik | 0:a1ad0eb8b619 | 500 | while(1) { |
| skyyoungsik | 0:a1ad0eb8b619 | 501 | GUI.pc_rx_update(); |
| skyyoungsik | 0:a1ad0eb8b619 | 502 | LED.Update(); |
| skyyoungsik | 0:a1ad0eb8b619 | 503 | RCV1.Update(); |
| skyyoungsik | 0:a1ad0eb8b619 | 504 | RCV2.Update(); |
| skyyoungsik | 0:a1ad0eb8b619 | 505 | RCV3.Update(); |
| skyyoungsik | 0:a1ad0eb8b619 | 506 | RCV4.Update(); |
| skyyoungsik | 0:a1ad0eb8b619 | 507 | RCV5.Update(); |
| skyyoungsik | 0:a1ad0eb8b619 | 508 | RCV6.Update(); |
| skyyoungsik | 0:a1ad0eb8b619 | 509 | EXT_UART.Receive(); |
| skyyoungsik | 0:a1ad0eb8b619 | 510 | EXT_UART.Trans(); |
| skyyoungsik | 0:a1ad0eb8b619 | 511 | current_system_time = MainTimer.read_us(); |
| skyyoungsik | 0:a1ad0eb8b619 | 512 | /// 500 Hz /// |
| skyyoungsik | 0:a1ad0eb8b619 | 513 | if(current_system_time - ex_millis_2ms >= 2000){ |
| skyyoungsik | 0:a1ad0eb8b619 | 514 | ex_millis_2ms = current_system_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 515 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 516 | /// 200 Hz /// |
| skyyoungsik | 0:a1ad0eb8b619 | 517 | if(current_system_time - ex_millis_5ms >= 5000){ |
| skyyoungsik | 0:a1ad0eb8b619 | 518 | ex_millis_5ms = current_system_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 519 | compFilter(); |
| skyyoungsik | 0:a1ad0eb8b619 | 520 | ex_millis_ahrs_time = current_system_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 521 | SAS_Algorithm(); |
| skyyoungsik | 0:a1ad0eb8b619 | 522 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 523 | /// 50 Hz /// |
| skyyoungsik | 0:a1ad0eb8b619 | 524 | if(current_system_time - ex_millis_20ms >= 20000){ |
| skyyoungsik | 0:a1ad0eb8b619 | 525 | ex_millis_20ms = current_system_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 526 | rc_cap_calibrate(); |
| skyyoungsik | 0:a1ad0eb8b619 | 527 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 528 | /// 20 Hz /// |
| skyyoungsik | 0:a1ad0eb8b619 | 529 | if(current_system_time - ex_millis_50ms >= 50000){ |
| skyyoungsik | 0:a1ad0eb8b619 | 530 | ex_millis_50ms = current_system_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 531 | /////// EXT UART TRANS /////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 532 | EXT_UART.roll = rollAngle; |
| skyyoungsik | 0:a1ad0eb8b619 | 533 | EXT_UART.pitch = pitchAngle; |
| skyyoungsik | 0:a1ad0eb8b619 | 534 | EXT_UART.yaw = yawAngle; |
| skyyoungsik | 0:a1ad0eb8b619 | 535 | EXT_UART.want_yaw = want_yaw; |
| skyyoungsik | 0:a1ad0eb8b619 | 536 | for(int i=0; i<8; i++){ |
| skyyoungsik | 0:a1ad0eb8b619 | 537 | EXT_UART.cap[i] = GUI.cap[i]; |
| skyyoungsik | 0:a1ad0eb8b619 | 538 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 539 | for(int i=0; i<8; i++){ |
| skyyoungsik | 0:a1ad0eb8b619 | 540 | EXT_UART.pwm[i] = GUI.pwm[i]; |
| skyyoungsik | 0:a1ad0eb8b619 | 541 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 542 | for(int i=0; i<8; i++){ |
| skyyoungsik | 0:a1ad0eb8b619 | 543 | EXT_UART.debug[i] = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 544 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 545 | EXT_UART.Refresh(); |
| skyyoungsik | 0:a1ad0eb8b619 | 546 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 547 | /// 10 Hz /// |
| skyyoungsik | 0:a1ad0eb8b619 | 548 | if(current_system_time - ex_millis_100ms >= 100000){ |
| skyyoungsik | 0:a1ad0eb8b619 | 549 | ex_millis_100ms = current_system_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 550 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 551 | /// 1 Hz /// |
| skyyoungsik | 0:a1ad0eb8b619 | 552 | if(current_system_time - ex_millis_1000ms >= 1000000){ |
| skyyoungsik | 0:a1ad0eb8b619 | 553 | ex_millis_1000ms = current_system_time; |
| skyyoungsik | 0:a1ad0eb8b619 | 554 | // printf("x: %f \t\ty: %f \t\t z: %f \t\t heading: %f \t\r\n", x, y, z, yawAngle); |
| skyyoungsik | 0:a1ad0eb8b619 | 555 | // 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); |
| skyyoungsik | 0:a1ad0eb8b619 | 556 | // 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); |
| skyyoungsik | 0:a1ad0eb8b619 | 557 | // printf("MAX x: %f \t\tMAX y: %f \t\t heading: %f \t\r\n", max_m_x, max_m_y, heading); |
| skyyoungsik | 0:a1ad0eb8b619 | 558 | // printf("MIN x: %f \t\tMIN y: %f \t\t heading: %f \t\r\n", min_m_x, min_m_y, heading); |
| skyyoungsik | 0:a1ad0eb8b619 | 559 | // 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); |
| skyyoungsik | 0:a1ad0eb8b619 | 560 | // printf("Roll: %f \t\tPitch: %f \t\t Yaw: %f\t\r\n", rollAngle, pitchAngle, yawAngle); |
| skyyoungsik | 0:a1ad0eb8b619 | 561 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 562 | |
| skyyoungsik | 0:a1ad0eb8b619 | 563 | |
| skyyoungsik | 0:a1ad0eb8b619 | 564 | |
| skyyoungsik | 0:a1ad0eb8b619 | 565 | |
| skyyoungsik | 0:a1ad0eb8b619 | 566 | /////////////////////// GUI ////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 567 | if(GUI.rx_bool() == true){ |
| skyyoungsik | 0:a1ad0eb8b619 | 568 | cnt ++; |
| skyyoungsik | 0:a1ad0eb8b619 | 569 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 570 | //////////////////////////////////////////// [M/S] Mode and State /////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 571 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 572 | GUI.Mode1 = 8 * sin( PI/180 + cnt/50.0) + 8; |
| skyyoungsik | 0:a1ad0eb8b619 | 573 | GUI.Mode2 = 8 * cos( PI/180 + cnt/50.0) + 8; |
| skyyoungsik | 0:a1ad0eb8b619 | 574 | GUI.MissionState = 8 * sin( PI/180 + cnt/50.0) + 8; |
| skyyoungsik | 0:a1ad0eb8b619 | 575 | GUI.CurrentMarker = 11 * cos( PI/180 + cnt/100.0) + 10; |
| skyyoungsik | 0:a1ad0eb8b619 | 576 | GUI.Bat1 = 50 * sin(PI/180 + cnt/50.0) + 50; // 21.6 ~ 24.6 ( 6cell ) |
| skyyoungsik | 0:a1ad0eb8b619 | 577 | GUI.Bat2 = 50 * cos(PI/180 + cnt/50.0) + 50; // 11.1 ~ 12.6 ( 3cell ) |
| skyyoungsik | 0:a1ad0eb8b619 | 578 | // [ Button State & Home Point Chksum & Marker Chksum ] is change automatically in "ROBOFRIEN_GUI.cpp" // |
| skyyoungsik | 0:a1ad0eb8b619 | 579 | // You Can not change this setting, just use it /// |
| skyyoungsik | 0:a1ad0eb8b619 | 580 | if(GUI.button[0] == true) {} |
| skyyoungsik | 0:a1ad0eb8b619 | 581 | else {} |
| skyyoungsik | 0:a1ad0eb8b619 | 582 | if(GUI.button[1] == true) {} |
| skyyoungsik | 0:a1ad0eb8b619 | 583 | else {} |
| skyyoungsik | 0:a1ad0eb8b619 | 584 | if(GUI.button[2] == true) {} |
| skyyoungsik | 0:a1ad0eb8b619 | 585 | else {} |
| skyyoungsik | 0:a1ad0eb8b619 | 586 | if(GUI.button[3] == true) {} |
| skyyoungsik | 0:a1ad0eb8b619 | 587 | else {} |
| skyyoungsik | 0:a1ad0eb8b619 | 588 | if(GUI.button[4] == true) {} |
| skyyoungsik | 0:a1ad0eb8b619 | 589 | else {} |
| skyyoungsik | 0:a1ad0eb8b619 | 590 | |
| skyyoungsik | 0:a1ad0eb8b619 | 591 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 592 | //////////////////////////////////////////////// [GPS] GPS ////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 593 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 594 | GUI.utc_time = cnt * 10; |
| skyyoungsik | 0:a1ad0eb8b619 | 595 | GUI.latitude = 35117030 + 10000 * sin( PI/180 + cnt/50.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 596 | GUI.longitude = 129087896 + 10000 * cos( PI/180 + cnt/50.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 597 | GUI.altitude = 22768 + 32768 * sin( PI/180 + cnt/50.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 598 | GUI.SatNum = 10 + 10 * sin( PI/180 + cnt/50.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 599 | |
| skyyoungsik | 0:a1ad0eb8b619 | 600 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 601 | ///////////////////////////////////////////////// AHRS ////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 602 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 603 | GUI.rollx100 = (rollAngle * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 604 | GUI.pitchx100 = (pitchAngle * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 605 | GUI.yawx100 = (yawAngle * 100); |
| skyyoungsik | 0:a1ad0eb8b619 | 606 | GUI.roll_ratex100 = roll_rate * 100; |
| skyyoungsik | 0:a1ad0eb8b619 | 607 | GUI.pitch_ratex100 = pitch_rate * 100; |
| skyyoungsik | 0:a1ad0eb8b619 | 608 | GUI.yaw_ratex100 = yaw_rate * 100; |
| skyyoungsik | 0:a1ad0eb8b619 | 609 | GUI.VXx100 = 1000 * sin(PI/180 + cnt/50.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 610 | GUI.VYx100 = 1000 * cos(PI/180 + cnt/50.0); |
| skyyoungsik | 0:a1ad0eb8b619 | 611 | GUI.VZx100 = 1000 * sin(PI/180 + cnt/50.0 + PI/4); |
| skyyoungsik | 0:a1ad0eb8b619 | 612 | |
| skyyoungsik | 0:a1ad0eb8b619 | 613 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 614 | ////////////////////////////////////////////// [C/P] CAP/PWM //////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 615 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 616 | |
| skyyoungsik | 0:a1ad0eb8b619 | 617 | |
| skyyoungsik | 0:a1ad0eb8b619 | 618 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 619 | /////////////////////////////////////////// [E/D] Extra & Debug ///////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 620 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 621 | GUI.DEBUGx100[0] = 9000 * sin(PI/180 + cnt/50.0) + 9000; |
| skyyoungsik | 0:a1ad0eb8b619 | 622 | GUI.DEBUGx100[1] += 100; |
| skyyoungsik | 0:a1ad0eb8b619 | 623 | if(GUI.DEBUGx100[1] >= 36000) GUI.DEBUGx100[1] = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 624 | GUI.DEBUGx100[2] = 25000 * sin(PI/180 + cnt/50.0) + 25000; |
| skyyoungsik | 0:a1ad0eb8b619 | 625 | GUI.DEBUGx100[2] = raw_mag_x * 100 + 180; |
| skyyoungsik | 0:a1ad0eb8b619 | 626 | |
| skyyoungsik | 0:a1ad0eb8b619 | 627 | |
| skyyoungsik | 0:a1ad0eb8b619 | 628 | GUI.DEBUGx100[4] = ax*100 + 10000; |
| skyyoungsik | 0:a1ad0eb8b619 | 629 | GUI.DEBUGx100[5] = ay*100 + 10000; |
| skyyoungsik | 0:a1ad0eb8b619 | 630 | GUI.DEBUGx100[6] = az*100 + 10000; |
| skyyoungsik | 0:a1ad0eb8b619 | 631 | GUI.DEBUGx100[4] = GUI.cal_accel_bias[0] * 100 + 10000; |
| skyyoungsik | 0:a1ad0eb8b619 | 632 | GUI.DEBUGx100[5] = GUI.cal_accel_bias[1] * 100 + 10000; |
| skyyoungsik | 0:a1ad0eb8b619 | 633 | GUI.DEBUGx100[6] = GUI.cal_accel_bias[2] * 100 + 10000; |
| skyyoungsik | 0:a1ad0eb8b619 | 634 | GUI.DEBUGx100[7] = MainTimer.read_us()/100000; |
| skyyoungsik | 0:a1ad0eb8b619 | 635 | |
| skyyoungsik | 0:a1ad0eb8b619 | 636 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 637 | /////////////////////////////////////////// Configuration /////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 638 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 639 | |
| skyyoungsik | 0:a1ad0eb8b619 | 640 | ///////////////////////////// -----------------DPN 1-------------------- //////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 641 | // You can set the this value from "Config.h" ( EEPROM_MODEL_TYPE1, EEPROM_MODEL_TYPE2_UP, EEPROM_MODEL_TYPE2_DOWN ) // |
| skyyoungsik | 0:a1ad0eb8b619 | 642 | |
| skyyoungsik | 0:a1ad0eb8b619 | 643 | ///////////////////////////// -----------------DPN 2-------------------- //////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 644 | // You can set the this value from "Config.h" ( MODEL_INFO, FIRMWARE_INFO ) // |
| skyyoungsik | 0:a1ad0eb8b619 | 645 | |
| skyyoungsik | 0:a1ad0eb8b619 | 646 | ///////////////////////////// -----------------DPN 3-------------------- //////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 647 | /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) /// |
| skyyoungsik | 0:a1ad0eb8b619 | 648 | /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value // |
| skyyoungsik | 0:a1ad0eb8b619 | 649 | |
| skyyoungsik | 0:a1ad0eb8b619 | 650 | ///////////////////////////// -----------------DPN 4-------------------- //////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 651 | /// You can use the [pwm_info , motor_min[4]] for calibration of motor pwm value // |
| skyyoungsik | 0:a1ad0eb8b619 | 652 | |
| skyyoungsik | 0:a1ad0eb8b619 | 653 | ///////////////////////////// -----------------DPN 5-------------------- //////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 654 | LED.HeadlightPeriod = GUI.headlight_period; |
| skyyoungsik | 0:a1ad0eb8b619 | 655 | LED.HeadlightDutyrate = GUI.headlight_dutyrate; |
| skyyoungsik | 0:a1ad0eb8b619 | 656 | LED.SidelightPeriod = GUI.sidelight_period; |
| skyyoungsik | 0:a1ad0eb8b619 | 657 | LED.SidelightDutyrate = GUI.sidelight_dutyrate; |
| skyyoungsik | 0:a1ad0eb8b619 | 658 | |
| skyyoungsik | 0:a1ad0eb8b619 | 659 | ///////////////////////////// -----------------DPN 6-------------------- //////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 660 | /// You can set the original value of receiver to [raw_cap] with scale down ( -127 ~ 127 ) /// |
| skyyoungsik | 0:a1ad0eb8b619 | 661 | /// You can use the [cap_min[8], cap_neu[8], cap_max[8]] for calibration of RC receiver value // |
| skyyoungsik | 0:a1ad0eb8b619 | 662 | GUI.raw_cap[0] = (RCV1.lpf_cap[0] - 150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 663 | GUI.raw_cap[1] = (RCV2.lpf_cap[1] - 150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 664 | GUI.raw_cap[2] = (RCV3.lpf_cap[2] - 150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 665 | GUI.raw_cap[3] = (RCV4.lpf_cap[3] - 150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 666 | GUI.raw_cap[4] = (RCV5.lpf_cap[4] - 150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 667 | GUI.raw_cap[5] = (RCV6.lpf_cap[5] - 150)*2; |
| skyyoungsik | 0:a1ad0eb8b619 | 668 | GUI.raw_cap[6] = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 669 | GUI.raw_cap[7] = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 670 | |
| skyyoungsik | 0:a1ad0eb8b619 | 671 | if(GUI.attitude_configuration_bool == true){ |
| skyyoungsik | 0:a1ad0eb8b619 | 672 | GUI.attitude_configuration_bool = false; |
| skyyoungsik | 0:a1ad0eb8b619 | 673 | // You can calibration of attitude (Roll, Pitch) // |
| skyyoungsik | 0:a1ad0eb8b619 | 674 | GUI.attitude_calibrate(accelData[0]*aRes,accelData[1]*aRes,(accelData[2]*aRes-1)); |
| skyyoungsik | 0:a1ad0eb8b619 | 675 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 676 | if(GUI.Compass_Calibration_Mode == 1){ |
| skyyoungsik | 0:a1ad0eb8b619 | 677 | // You can calibration of compass (Yaw)// |
| skyyoungsik | 0:a1ad0eb8b619 | 678 | if(ex_Compass_Calibration_Mode == 0){ |
| skyyoungsik | 0:a1ad0eb8b619 | 679 | magnetic_offset_reset(); |
| skyyoungsik | 0:a1ad0eb8b619 | 680 | }else{ |
| skyyoungsik | 0:a1ad0eb8b619 | 681 | //// calibrating ... ///// |
| skyyoungsik | 0:a1ad0eb8b619 | 682 | magnetic_calibration(); |
| skyyoungsik | 0:a1ad0eb8b619 | 683 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 684 | }else if(GUI.Compass_Calibration_Mode == 2){ |
| skyyoungsik | 0:a1ad0eb8b619 | 685 | // You can finish the calibration of compass |
| skyyoungsik | 0:a1ad0eb8b619 | 686 | //// write to eeprom //// |
| skyyoungsik | 0:a1ad0eb8b619 | 687 | GUI.Compass_Calibration_Mode = 0; |
| skyyoungsik | 0:a1ad0eb8b619 | 688 | GUI.write_compass_setting_to_eeprom(); |
| skyyoungsik | 0:a1ad0eb8b619 | 689 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 690 | ex_Compass_Calibration_Mode = GUI.Compass_Calibration_Mode; |
| skyyoungsik | 0:a1ad0eb8b619 | 691 | |
| skyyoungsik | 0:a1ad0eb8b619 | 692 | ///////////////////////////// -----------------DPN 7-------------------- //////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 693 | /// You can use the [limit_rollx100, limit_pitchx100, limit_roll_rate, limit_pitch_rate, limit_yaw_rate] for limit the angle and angular velocity // |
| skyyoungsik | 0:a1ad0eb8b619 | 694 | |
| skyyoungsik | 0:a1ad0eb8b619 | 695 | ///////////////////////////// -----------------DPN 8-------------------- //////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 696 | /// You can use the [gain_px100[20], gain_dx100[20], gain_ix100[20]] for gain tuning // |
| skyyoungsik | 0:a1ad0eb8b619 | 697 | |
| skyyoungsik | 0:a1ad0eb8b619 | 698 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 699 | //////////////////////////////////////////////// Refresh //////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 700 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 701 | GUI.Refresh(); |
| skyyoungsik | 0:a1ad0eb8b619 | 702 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 703 | |
| skyyoungsik | 0:a1ad0eb8b619 | 704 | //////////////////////////////////////////////////////////////////////////////////// |
| skyyoungsik | 0:a1ad0eb8b619 | 705 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 706 | } |
| skyyoungsik | 0:a1ad0eb8b619 | 707 | |
| skyyoungsik | 0:a1ad0eb8b619 | 708 | |
| skyyoungsik | 0:a1ad0eb8b619 | 709 | |
| skyyoungsik | 0:a1ad0eb8b619 | 710 | |
| skyyoungsik | 0:a1ad0eb8b619 | 711 |