Atsumi Toda
ドローン用計測制御基板の作り方 vol.1 ハードウェア編 p.16掲載 相補フィルタによる姿勢角推定
Dependencies: mbed MPU6050_alter
Revision 0:06770dc62477, committed 2019-12-30
- Comitter:
- Joeatsumi
- Date:
- Mon Dec 30 08:51:14 2019 +0000
- Commit message:
- 20191230
Changed in this revision
diff -r 000000000000 -r 06770dc62477 MPU6050.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Mon Dec 30 08:51:14 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Joeatsumi/code/MPU6050_alter/#44c458576810
diff -r 000000000000 -r 06770dc62477 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Dec 30 08:51:14 2019 +0000
@@ -0,0 +1,172 @@
+//==================================================
+//Atitude estimation (compensated filter)
+//MPU board: mbed LPC1768
+//Accelerometer +Gyro sensor : GY-521
+//2019/10/11 A.Toda
+//==================================================
+#include "mbed.h"
+#include "MPU6050.h"
+
+//==================================================
+#define RAD_TO_DEG 57.2957795f // 180 / π
+#define MAX_MEAN_COUNTER 100
+#define ACC_X 1.3//offset of x-axi accelerometer
+//==================================================
+
+//Timer interrupt
+Ticker flipper;
+
+//Port Setting
+MPU6050 mpu(p9, p10); //Accelerometer + Gyro
+ //(SDA,SCLK)
+
+Serial pc(USBTX, USBRX); //UART
+
+//==================================================
+//Accelerometer and gyro data
+//==================================================
+double acc[3]; //variables for accelerometer
+double gyro[3]; //variables for gyro
+
+double offset_gyro_x=0.0;
+double offset_gyro_y=0.0;
+
+double sum_gyro_x=0.0;
+double sum_gyro_y=0.0;
+
+double threshold_acc,threshold_acc_ini;
+
+//==================================================
+//Atitude data
+//==================================================
+double roll_and_pitch_acc[2];//atitude from acceleromter
+double roll_and_pitch[2];//atitude from gyro and acceleromter
+
+//==================================================
+//Gyro and accelerometer functions
+//==================================================
+//get data
+void aquisition_sensor_values(double *a,double *g){
+
+ float ac[3],gy[3];
+
+ mpu.getAccelero(ac);//get acceleration (Accelerometer)
+ //x_axis acc[0]
+ //y_axis acc[1]
+ //z_axis acc[2]
+ mpu.getGyro(gy); //get rate of angle(Gyro)
+ //x_axis gyro[0]
+ //y_axis gyro[1]
+ //z_axis gyro[2]
+
+ //Invertion for direction of Accelerometer axis
+ ac[0]*=(-1.0);
+ ac[0]+=ACC_X;
+
+ ac[2]*=(-1.0);
+
+ //Unit convertion of rate of angle(radian to degree)
+ gy[0]*=RAD_TO_DEG;
+ gy[0]*=(-1.0);
+
+ gy[1]*=RAD_TO_DEG;
+
+ gy[2]*=RAD_TO_DEG;
+ gy[2]*=(-1.0);
+
+ for(int i=0;i<3;i++){
+ a[i]=double(ac[i]);
+ g[i]=double(gy[i]);
+ }
+ g[0]-=offset_gyro_x;//offset rejection
+ g[1]-=offset_gyro_y;//offset rejection
+
+ return;
+
+}
+
+//calculate offset of gyro
+void offset_calculation_for_gyro(){
+
+ //Accelerometer and gyro setting
+ mpu.setAcceleroRange(0);//acceleration range is +-2G
+ mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
+
+ //calculate offset of gyro
+ for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){
+ aquisition_sensor_values(acc,gyro);
+ sum_gyro_x+=gyro[0];
+ sum_gyro_y+=gyro[1];
+ wait(0.01);
+ }
+
+ offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER;
+ offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER;
+
+ return;
+}
+
+//atitude calculation from acceleromter
+void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){
+
+ //roll_and_pitch[0] = atan2(a[1], a[2])*RAD_TO_DEG;//roll
+ roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll
+ //roll_and_pitch[1] = atan(a[0]/sqrt( a[1]*a[1]+a[2]*a[2] ) )*RAD_TO_DEG;//pitch
+ roll_and_pitch[1] = atan2(a[0],sqrt( a[1]*a[1]+a[2]*a[2] ) )*RAD_TO_DEG;//pitch
+
+ return;
+}
+
+//atitude calculation
+void atitude_update(){
+
+ aquisition_sensor_values(acc,gyro);
+
+ roll_and_pitch[0]+=gyro[0]*0.01;
+ roll_and_pitch[1]+=gyro[1]*0.01;
+
+ threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
+
+ if((threshold_acc>=0.9*threshold_acc_ini)
+ &&(threshold_acc<=1.1*threshold_acc_ini)){
+
+ atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc);
+ roll_and_pitch[0] = 0.98*roll_and_pitch[0] + 0.02*roll_and_pitch_acc[0];
+ //roll_and_pitch[1] = 0.98*roll_and_pitch[1] + 0.02*roll_and_pitch_acc[1];
+ roll_and_pitch[1] = roll_and_pitch_acc[1];
+
+ }else{}
+
+ pc.printf("roll=%f pitch=%f\r\n",roll_and_pitch[0],roll_and_pitch[1]);
+
+
+ return;
+
+}
+
+//==================================================
+//Main
+//==================================================
+int main() {
+
+ //UART initialization
+ pc.baud(115200);
+
+ //gyro and accelerometer initialization
+ offset_calculation_for_gyro();
+
+ //identify initilal atitude
+ aquisition_sensor_values(acc,gyro);
+ atitude_estimation_from_accelerometer(acc,roll_and_pitch);
+
+ threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
+
+ //Ticker
+ flipper.attach(&atitude_update, 0.01);
+
+ //while
+ while(1) {
+
+ wait(0.01);
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 06770dc62477 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Dec 30 08:51:14 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file