20160815 計器最新版
Dependencies: SDFileSystem mbed
Fork of keiki2016verRtos by
main.cpp
00001 //計器プログラム 00002 #include "mbed.h" 00003 #include "Fusokukei.h" 00004 #include "MPU6050.h" 00005 #include "SDFileSystem.h" 00006 00007 #define KX_VALUE_MIN 0.4 00008 #define KX_VALUE_MAX 0.8 00009 #define SOUDA_DATAS_NUM 13 00010 #define WRITE_DATAS_NUM 20 00011 #define MPU_LOOP_TIME 0.01 00012 #define AIR_LOOP_TIME 0.01 00013 #define WRITE_DATAS_LOOP_TIME 1 00014 #define ROLL_R_MAX_DEG 2 00015 #define ROLL_L_MAX_DEG 2 00016 #define MPU_DELT_MIN 250 00017 #define SD_WRITE_NUM 10 00018 #define INIT_SERVO_PERIOD_MS 20 00019 00020 Serial pc(USBTX,USBRX); 00021 Serial soudaSerial(p13,p14); 00022 Serial twe(p9,p10); 00023 Ticker writeDatasTicker; 00024 Timer writeTimer; 00025 00026 InterruptIn FusokukeiPin(p30); 00027 Ticker FusokukeiTicker; 00028 Fusokukei air; 00029 volatile int air_kaitensu= 0; 00030 00031 float sum = 0; 00032 uint32_t sumCount = 0; 00033 MPU6050 mpu6050; 00034 Timer t; 00035 Ticker mpu6050Ticker; 00036 00037 AnalogIn kx_X(p17); 00038 AnalogIn kx_Y(p16); 00039 AnalogIn kx_Z(p15); 00040 float KX_X,KX_Y,KX_Z; 00041 00042 DigitalOut RollAlarmR(p20); 00043 DigitalOut RollAlarmL(p19); 00044 DigitalOut led(LED1); 00045 DigitalOut led2(LED2); 00046 00047 SDFileSystem sd(p5, p6, p7, p8, "sd"); 00048 FILE* fp; 00049 00050 PwmOut kisokuServo(p22); 00051 PwmOut geikakuServo(p26); 00052 00053 char soudaDatas[SOUDA_DATAS_NUM]; 00054 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM]; 00055 volatile int write_datas_index = 0; 00056 00057 void air_countUp(); 00058 void call_calcAirSpeed(); 00059 void init(); 00060 void FusokukeiInit(); 00061 void MpuInit(); 00062 void mpuProcessing(); 00063 void SdInit(); 00064 void DataReceiveFromSouda(); 00065 void WriteDatas(); 00066 float calcAttackAngle(); 00067 float calcKXdeg(float x); 00068 00069 void air_countUp(){ 00070 air_kaitensu++; 00071 } 00072 00073 void call_calcAirSpeed(){ 00074 air.calcAirSpeed(air_kaitensu); 00075 air_kaitensu = 0; 00076 } 00077 00078 void init(){ 00079 twe.baud(115200); 00080 //writeTimer.start(); 00081 kisokuServo.period_ms(INIT_SERVO_PERIOD_MS); 00082 geikakuServo.period_ms(INIT_SERVO_PERIOD_MS); 00083 FusokukeiInit(); 00084 MpuInit(); 00085 SdInit(); 00086 //writeDatasTicker.attach(&WriteDatas,1); 00087 //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq); 00088 } 00089 00090 void FusokukeiInit(){ 00091 FusokukeiPin.rise(air_countUp); 00092 FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME); 00093 } 00094 00095 void MpuInit(){ 00096 i2c.frequency(400000); // use fast (400 kHz) I2C 00097 t.start(); 00098 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 00099 if (whoami == 0x68) { // WHO_AM_I should always be 0x68 00100 wait(1); 00101 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values 00102 wait(1); 00103 if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { 00104 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration 00105 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 00106 mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature 00107 wait(2); 00108 } else { 00109 } 00110 } else { 00111 //pc.printf("out\n\r"); // Loop forever if communication doesn't happen 00112 } 00113 } 00114 00115 double calcPulse(int deg){ 00116 return (0.0006+(deg/180.0)*(0.00235-0.00045)); 00117 00118 } 00119 00120 void mpuProcessing(){ 00121 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt 00122 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values 00123 mpu6050.getAres(); 00124 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set 00125 ay = (float)accelCount[1]*aRes - accelBias[1]; 00126 az = (float)accelCount[2]*aRes - accelBias[2]; 00127 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values 00128 mpu6050.getGres(); 00129 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set 00130 gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; 00131 gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; 00132 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values 00133 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade 00134 } 00135 Now = t.read_us(); 00136 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update 00137 lastUpdate = Now; 00138 sum += deltat; 00139 sumCount++; 00140 if(lastUpdate - firstUpdate > 10000000.0f) { 00141 beta = 0.04; // decrease filter gain after stabilized 00142 zeta = 0.015; // increasey bias drift gain after stabilized 00143 } 00144 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); 00145 delt_t = t.read_ms() - count; 00146 if (delt_t > MPU_DELT_MIN) { 00147 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); 00148 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 00149 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); 00150 pitch *= 180.0f / PI; 00151 yaw *= 180.0f / PI; 00152 roll *= 180.0f / PI; 00153 myled= !myled; 00154 count = t.read_ms(); 00155 sum = 0; 00156 sumCount = 0; 00157 } 00158 } 00159 00160 void SdInit(){ 00161 mkdir("/sd/mydir", 0777); 00162 fp = fopen("/sd/mydir/sdtest2.csv", "w"); 00163 if(fp == NULL) { 00164 error("Could not open file for write\n"); 00165 } 00166 fprintf(fp, "Hello fun SD Card World!\n\r"); 00167 fclose(fp); 00168 } 00169 00170 void DataReceiveFromSouda(){ 00171 led2 = !led2; 00172 //pc.printf("received\n\r"); 00173 for(int i = 0; i < SOUDA_DATAS_NUM; i++){ 00174 soudaDatas[i] = soudaSerial.getc(); 00175 } 00176 } 00177 00178 void SDprintf(){ 00179 fp = fopen("/sd/mydir/sdtest.csv", "a"); 00180 if(fp == NULL) { 00181 error("Could not open file for write\n"); 00182 } 00183 for(int i = 0; i < SD_WRITE_NUM; i++){ 00184 for(int j = 0; j < WRITE_DATAS_NUM; j++){ 00185 fprintf(fp,"%f,", writeDatas[i][j]); 00186 } 00187 } 00188 fprintf(fp,"\n\r"); 00189 fclose(fp); 00190 } 00191 00192 void WriteDatas(){ 00193 int i; 00194 for(i = 0; i < SOUDA_DATAS_NUM; i++){ 00195 //writeDatas[write_datas_index][i] = 0.0; 00196 writeDatas[write_datas_index][i] = (float)soudaDatas[i]; 00197 } 00198 writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read()); 00199 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read()); 00200 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read()); 00201 writeDatas[write_datas_index][i++] = pitch; 00202 writeDatas[write_datas_index][i++] = roll; 00203 writeDatas[write_datas_index][i++] = yaw; 00204 writeDatas[write_datas_index][i++] = airSpeed; 00205 //writeDatas[write_datas_index][i++] = writeTimer.read(); 00206 //for(i = 0; i < WRITE_DATAS_NUM; i++){ 00207 // pc.printf("%f ", writeDatas[write_datas_index][i]); 00208 // twe.printf("%f,", writeDatas[write_datas_index][i]); 00209 // } 00210 // pc.printf("\n\r"); 00211 // twe.printf("\n\r"); 00212 if(write_datas_index == SD_WRITE_NUM-1){ 00213 SDprintf(); 00214 write_datas_index=0; 00215 } 00216 else{ 00217 write_datas_index++; 00218 } 00219 for(int i = 0; i < SOUDA_DATAS_NUM; i++){ 00220 pc.printf("%i ",soudaDatas[i]); 00221 twe.printf("%i ",soudaDatas[i]); 00222 } 00223 //pc.printf("\n\r"); 00224 twe.printf("\n\r"); 00225 twe.printf("%f,%f,%f\n\r",pitch,roll,yaw); 00226 twe.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); 00227 twe.printf("%f\n\r",airSpeed); 00228 pc.printf("%f,%f,%f\n\r",pitch,roll,yaw); 00229 //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); 00230 pc.printf("%f\n\r",airSpeed); 00231 //SDprintf(); 00232 } 00233 00234 void WriteDatasF(){ 00235 pc.printf("airSpeed:%f\n\r",airSpeed); 00236 } 00237 00238 float calcKXdeg(float x){ 00239 return -310.54*x+156.65; 00240 } 00241 00242 float calcAttackAngle(){ 00243 return pitch-calcKXdeg(kx_Z.read()); 00244 } 00245 00246 void RollAlarm(){ 00247 if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){ 00248 RollAlarmL = 1; 00249 } 00250 else{ 00251 RollAlarmL = 0; 00252 } 00253 00254 if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){ 00255 RollAlarmR = 1; 00256 } 00257 else{ 00258 RollAlarmR = 0; 00259 } 00260 } 00261 00262 void WriteServo(){ 00263 //kisokuServo.pulsewidth(calcPulse(airSpeed*10)); 00264 kisokuServo.pulsewidth(calcPulse(9*airSpeed)); 00265 if(pitch<0){ 00266 geikakuServo.pulsewidth(calcPulse(0)); 00267 } 00268 else{ 00269 geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0))); 00270 } 00271 //pc.printf("a:%f",airSpeed); 00272 //pc.printf("p:%f\r\n",pitch); 00273 //kisokuServo.pulsewidth(calcPulse(0)); 00274 //geikakuServo.pulsewidth(calcPulse(0)); 00275 } 00276 00277 int main(){ 00278 init(); 00279 while(1){ 00280 pc.printf("test\n\r"); 00281 mpuProcessing(); 00282 RollAlarm(); 00283 DataReceiveFromSouda(); 00284 WriteDatas(); 00285 WriteServo(); 00286 } 00287 }
Generated on Sat Jul 16 2022 08:38:47 by 1.7.2