albatross
20160815 計器最新版
Dependencies: SDFileSystem mbed
Fork of
keiki2016verRtos
by 
albatross
Diff: main.cpp
- Revision:
- 3:8dc516be2e7e
- Parent:
- 2:5a1a638f5fe6
- Child:
- 4:a863a092141c
--- a/main.cpp Thu Feb 25 09:13:46 2016 +0000
+++ b/main.cpp Thu Mar 24 06:42:22 2016 +0000
@@ -7,17 +7,20 @@
#define KX_VALUE_MIN 0.4
#define KX_VALUE_MAX 0.8
#define SOUDA_DATAS_NUM 13
-#define WRITE_DATAS_NUM 28
+#define WRITE_DATAS_NUM 21
#define MPU_LOOP_TIME 0.01
#define AIR_LOOP_TIME 0.01
-#define WRITE_DATAS_LOOP_TIME 0.5
-#define ROLL_R_MAX_DEG 5
-#define ROLL_L_MAX_DEG 5
+#define WRITE_DATAS_LOOP_TIME 1
+#define ROLL_R_MAX_DEG 2
+#define ROLL_L_MAX_DEG 2
+#define MPU_DELT_MIN 500
+#define SD_WRITE_NUM 10
Serial pc(USBTX,USBRX);
Serial soudaSerial(p13,p14);
Serial twe(p9,p10);
Ticker writeDatasTicker;
+Timer writeTimer;
InterruptIn FusokukeiPin(p30);
Ticker FusokukeiTicker;
@@ -33,6 +36,7 @@
AnalogIn kx_X(p17);
AnalogIn kx_Y(p16);
AnalogIn kx_Z(p15);
+float KX_X,KX_Y,KX_Z;
DigitalOut RollAlarmR(p20);
DigitalOut RollAlarmL(p19);
@@ -43,7 +47,8 @@
FILE* fp;
char soudaDatas[SOUDA_DATAS_NUM];
-char writeDatas[WRITE_DATAS_NUM];
+float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
+volatile int write_datas_index = 0;
void air_countUp();
void call_calcAirSpeed();
@@ -68,10 +73,11 @@
void init(){
twe.baud(115200);
+ writeTimer.start();
FusokukeiInit();
MpuInit();
SdInit();
- writeDatasTicker.attach(&WriteDatas,0.25);
+ //writeDatasTicker.attach(&WriteDatas,1);
//soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq);
}
@@ -102,41 +108,42 @@
void mpuProcessing(){
if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
- mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
- mpu6050.getAres();
- ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
- ay = (float)accelCount[1]*aRes - accelBias[1];
- az = (float)accelCount[2]*aRes - accelBias[2];
- mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
- mpu6050.getGres();
- gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
- gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
- gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
- tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
- temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
- }
- Now = t.read_us();
- deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
- lastUpdate = Now;
- sum += deltat;
- sumCount++;
- if(lastUpdate - firstUpdate > 10000000.0f) {
- beta = 0.04; // decrease filter gain after stabilized
- zeta = 0.015; // increasey bias drift gain after stabilized
- }
- mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
- delt_t = t.read_ms() - count;
- if (delt_t > 800) { // update LCD once per half-second independent of read rate
- 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]);
- pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
- 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]);
- pitch *= 180.0f / PI;
- yaw *= 180.0f / PI;
- roll *= 180.0f / PI;
- myled= !myled;
- count = t.read_ms();
- sum = 0;
- sumCount = 0;
+ mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
+ mpu6050.getAres();
+ ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ ay = (float)accelCount[1]*aRes - accelBias[1];
+ az = (float)accelCount[2]*aRes - accelBias[2];
+ mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
+ mpu6050.getGres();
+ gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
+ gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
+ tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
+ temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+ }
+ Now = t.read_us();
+ deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ lastUpdate = Now;
+ sum += deltat;
+ sumCount++;
+ if(lastUpdate - firstUpdate > 10000000.0f) {
+ beta = 0.04; // decrease filter gain after stabilized
+ zeta = 0.015; // increasey bias drift gain after stabilized
+ }
+ mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+ delt_t = t.read_ms() - count;
+ if (delt_t > MPU_DELT_MIN) {
+ 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]);
+ pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ 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]);
+ pitch *= 180.0f / PI;
+ yaw *= 180.0f / PI;
+ roll *= 180.0f / PI;
+ pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
+ myled= !myled;
+ count = t.read_ms();
+ sum = 0;
+ sumCount = 0;
}
}
@@ -158,31 +165,69 @@
}
}
-void WriteDatas(){
- pc.printf("write\n\r");
- for(int i = 0; i < SOUDA_DATAS_NUM; i++){
- pc.printf("%i ",soudaDatas[i]);
- twe.printf("%i ",soudaDatas[i]);
- }
- pc.printf("\n\r");
- twe.printf("\n\r");
- twe.printf("%f,%f,%f\n\r",pitch,roll,yaw);
- // twe.printf("%f,%f,%f\n\r",gx,gy,gz);
-// twe.printf("%f\n\r",airSpeed);
- pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
- pc.printf("%f,%f,%f\n\r",gx,gy,gz);
- pc.printf("%f\n\r",airSpeed);
+void SDprintf(){
fp = fopen("/sd/mydir/sdtest.csv", "a");
if(fp == NULL) {
error("Could not open file for write\n");
}
- for(int i = 0; i < SOUDA_DATAS_NUM; i++){
- fprintf(fp,"%i ",soudaDatas[i]);
+ for(int i = 0; i < SD_WRITE_NUM; i++){
+ for(int j = 0; j < WRITE_DATAS_NUM; j++){
+ fprintf(fp,"%f ", writeDatas[i][j]);
+ if(i%7==0){
+ fprintf(fp,"\n\r");
+ }
+ }
+ }
+ //for(int i = 0; i < SOUDA_DATAS_NUM; i++){
+// fprintf(fp,"%i ",soudaDatas[i]);
+// }
+// fprintf(fp, "p:%f,r:%f,y:%f\n",pitch,roll,yaw);
+// fprintf(fp, "gx:%f,gy:%f,gz:%f\n",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
+// fprintf(fp, "as:%f\n",airSpeed);
+ fclose(fp);
+}
+
+void WriteDatas(){
+ int i;
+ for(i = 0; i < SOUDA_DATAS_NUM; i++){
+ writeDatas[write_datas_index][i] = (float)soudaDatas[i];
}
- fprintf(fp, "p:%f,r:%f,y:%f\n",pitch,roll,yaw);
- fprintf(fp, "gx:%f,gy:%f,gz:%f\n",gx,gy,gz);
- fprintf(fp, "as:%f\n",airSpeed);
- fclose(fp);
+ writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
+ writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
+ writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
+ writeDatas[write_datas_index][i++] = pitch;
+ writeDatas[write_datas_index][i++] = roll;
+ writeDatas[write_datas_index][i++] = yaw;
+ writeDatas[write_datas_index][i++] = airSpeed;
+ writeDatas[write_datas_index][i++] = writeTimer.read();
+ for(i = 0; i < WRITE_DATAS_NUM; i++){
+ pc.printf("%f ", writeDatas[write_datas_index][i]);
+ twe.printf("%f ", writeDatas[write_datas_index][i]);
+ if(i%7==0){
+ pc.printf("\n\r");
+ twe.printf("\n\r");
+ }
+ }
+ if(write_datas_index == SD_WRITE_NUM){
+ SDprintf();
+ write_datas_index=0;
+ }
+ else{
+ write_datas_index++;
+ }
+ //for(int i = 0; i < SOUDA_DATAS_NUM; i++){
+// pc.printf("%i ",soudaDatas[i]);
+// twe.printf("%i ",soudaDatas[i]);
+// }
+// pc.printf("\n\r");
+// twe.printf("\n\r");
+// twe.printf("%f,%f,%f\n\r",pitch,roll,yaw);
+// twe.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
+// twe.printf("%f\n\r",airSpeed);
+// pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
+// pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
+// pc.printf("%f\n\r",airSpeed);
+// SDprintf();
}
float calcKXdeg(float x){
@@ -193,12 +238,6 @@
return pitch-calcKXdeg(kx_Z.read());
}
-void kxRead(){
- gx = kx_X.read();
- gy = kx_Y.read();
- gz = kx_Z.read();
-}
-
void RollAlarm(){
if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
RollAlarmL = 1;
@@ -219,8 +258,8 @@
init();
while(1){
mpuProcessing();
- kxRead();
RollAlarm();
DataReceiveFromSouda();
+ WriteDatas();
}
}
\ No newline at end of file