solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 23:4a490fa8bf4a
- Parent:
- 22:da9893b71f24
- Child:
- 24:6ab61527855d
--- a/main.cpp Mon Feb 15 06:49:37 2021 +0000 +++ b/main.cpp Mon Feb 15 06:57:00 2021 +0000 @@ -58,8 +58,8 @@ Timer t; int ch1, ch2; -int channels[16]; -//float rc1, rc2; // channels[16]の1,2要素めにそれぞれ移行ずみ +float rc[16]; +//float rc1, rc2; // rc[16]の1,2要素めにそれぞれ移行ずみ double pitch = 0.0; double roll = 0.0; double yaw = 0.0; @@ -126,11 +126,9 @@ for (int i =0 ; i < 16;i ++){ int data; float mapped_data; - data = int(sbus.getData(i)); - mapped_data = LP_rc * float(map(data,368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * channels[i]; // mapped input - channels[i] = int(mapped_data); // channelの値をmapしたものを格納 + rc[i] = LP_rc * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc[i]; // mapped input } - pc.printf("data: %d",sbus.getData(1)); + pc.printf("data: %d",rc[1]); } // gx gy gz ax ay az @@ -157,21 +155,21 @@ //rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; int pwmMax = 1800; int pwmMin = 1200; - out1 = map((int)(channels[1]*1000.0f),-1000,1000,pwmMin,pwmMax); + out1 = map((int)(rc[1]*1000.0f),-1000,1000,pwmMin,pwmMax); if(out1<pwmMin) { out1 = pwmMin; }; if(out1>pwmMax) { out1 = pwmMax; }; - out2 = map((int)(channels[2]*1000.0f),-1000,1000,pwmMin,pwmMax); + out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax); if(out2<pwmMin) { out2 = pwmMin; }; if(out2>pwmMax) { out2 = pwmMax; }; - //test_control.setSetPoint(channels[1]); + //test_control.setSetPoint(rc[1]); test_control.setSetPoint(0.0); // 仮(実際にはsbusの読み込みなど) pc.printf("roll%f ", roll*180.0/pi); //pc.printf("out1:%d\n ", out1); @@ -181,7 +179,7 @@ MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza pc.printf("time%f \r\n", (tend-tstart)); test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう - out1 = test_control.compute(); + //out1 = test_control.compute(); pc.printf("out1:%d ",out1); servoRight.pulsewidth_us(out1); }