HAPSRG
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:
- 38:acc7cdcf56dd
- Parent:
- 37:dad55cf05e3d
- Child:
- 40:869f3791a3e2
- Child:
- 41:4c02fcf105a9
--- a/main.cpp Wed Mar 03 09:32:10 2021 +0000
+++ b/main.cpp Mon Mar 22 02:34:47 2021 +0000
@@ -1,82 +1,394 @@
#include "mbed.h"
#include "PIDcontroller.h"
#include "SBUS.hpp"
-//#include <MadgwickAHRS.h>
#include "LoopTicker.hpp"
#include "MPU6050.h"
-#include <I2Cdev.h>
+#include "MAG3110.h"
+#include "I2Cdev.h"
#include "FastPWM.h"
#include "Matrix.h"
#include "MatrixMath.h"
-#include <math.h>
+#include "math.h"
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU_ADDRESS 0x68
#define pi 3.141562
#define ACCEL_FSR MPU6050_ACCEL_FS_8
-#define ACCEL_SSF 36500.0
+#define ACCEL_SSF 4096.0
#define GYRO_FSR MPU6050_GYRO_FS_250
#define GYRO_SSF 131.0
#define MPU6050_LPF MPU6050_DLPF_BW_256
+#define gyro_th 20.0
+#define PID_dt 0.015
+#define servoPwmMax 1800.0
+#define servoPwmMin 1200.0
+#define motorPwmMax 2000.0
+#define motorPwmMin 1100.0
+#define pitch_align 8.0*3.141562/180.0
+#define roll_align -2.8*3.141562/180.0
MPU6050 accelgyro;
-//Madgwick MadgwickFilter;
+MAG3110 mag(PB_9,PB_8);
SBUS sbus(PD_5, PD_6);
Serial pc(USBTX, USBRX);
Serial sd(PG_14,PG_9);
-DigitalOut led1(LED1);
-DigitalOut led3(LED3);
-
+DigitalIn userButton(USER_BUTTON);
FastPWM servoRight(PE_9);
FastPWM servoLeft(PE_11);
-FastPWM servoThrust(PA_0);
-const double PID_dt = 0.015;
+FastPWM servoThrust(PE_13);
PID pitchPID(5.0, 0,0,PID_dt); //rad
-PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
+PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
PID rollPID(5.0,0.0,0.0,PID_dt);
-PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
+PID rollratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
Timer t;
Matrix qhat(4,1);
Matrix Phat(4,4);
Matrix Qgyro(3,3);
Matrix Racc(3,3);
-
+Matrix Rmag(3,3);
+Matrix D(3,3);
-int loop_count = 1;
+int loop_count = 0;
float att_dt = 0.01;
float rc[16];
-float roll_ac;
-double pitch = 0.0;
-double roll = 0.0;
-double yaw = 0.0;
+float pitch = 0.0;
+float roll = 0.0;
+float yaw = 0.0;
int16_t ax, ay, az;
int16_t gx, gy, gz;
-double acc_x,acc_y,acc_z;
-double gyro_x,gyro_y,gyro_z;
+MotionSensorDataUnits mdata;
+float magval[3] = {1.0,0.0,0.0};
+float acc_x,acc_y,acc_z;
+float dynacc_x,dynacc_y,dynacc_z;
+float gyro_x,gyro_y,gyro_z;
+float mag_x,mag_y,mag_z;
int out1, out2;
float scaledServoOut[3]= {0,0};
float scaledMotorOut[1]= {-1};
float servoOut[3] = {1500.0,1500.0};
float motorOut[1] = {1100.0};
-float servoPwmMax = 1800.0;
-float servoPwmMin = 1200.0;
-float motorPwmMax = 2000.0;
-float motorPwmMin = 1100.0;
-int offsetVal[6] = {0,0,0,-600,570,265};
+
+float accnorm;
+float magnorm;
+float accrefnorm;
+float magrefnorm;
+
+float val_thmg = 0.0;
+float accref[3] = {0, 0, -0.98};
+float magref[3] = {0.65, 0, 0.75};
+
+int agoffset[6] = {0, 0, 0, -123, -575, -1};
+float magbias[4] = {-143.593872, 125.311264, -161.226898, 27.998077};
-const double pitch_align = 0.0;
-const double roll_align = -0.0;
-
+void writeSdcard()
+{
+ //magcal.getExtremes(&magmin[0],&magmax[0]);
+ //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]);
+ //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]);
+ sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
+ //pc.printf("%d \r\n",userButton.read());
+ //pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
+ //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
+ //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
+ //pc.printf("%f %f %f %f %f : %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
+ //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z);
+}
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
-void writeSdcard()
+void calcMagRef(float mx, float my, float mz){
+ float q0 = qhat.getNumber( 1, 1 );
+ float q1 = qhat.getNumber( 2, 1 );
+ float q2 = qhat.getNumber( 3, 1 );
+ float q3 = qhat.getNumber( 4, 1 );
+ float hx = 2.0f * (mx * (0.5f - q2*q2 - q3*q3) + my * (q1*q2 - q0*q3) + mz * (q1*q3 + q0*q2));
+ float hy = 2.0f * (mx * (q1*q2 + q0*q3) + my * (0.5f - q1*q1 - q3*q3) + mz * (q2*q3 - q0*q1));
+ float bx = sqrt(hx * hx + hy * hy);
+ float bz = 2.0f * (mx * (q1*q3 - q0*q2) + my * (q2*q3 + q0*q1) + mz * (0.5f - q1*q1 - q2*q2));
+ magref[0] = bx;
+ magref[1] = 0.0;
+ magref[2] = bz;
+}
+
+
+void getIMUval(){
+ // gx gy gz ax ay az
+ accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
+ ax = ax - agoffset[0];
+ ay = ay - agoffset[1];
+ az = -az - agoffset[2];
+ gx = gx - agoffset[3];
+ gy = gy - agoffset[4];
+ gz = -gz - agoffset[5];
+ // 加速度値を分解能で割って加速度(G)に変換する
+ acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
+ acc_y = float(ay) / ACCEL_SSF;
+ acc_z = float(az) / ACCEL_SSF;
+ // 角速度値を分解能で割って角速度(rad per sec)に変換する
+ gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
+ gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
+ gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
+ mag.getAxis(mdata); // flush the magnetmeter
+ magval[0] = (mdata.x - magbias[0]);
+ magval[1] = (mdata.y - magbias[1]);
+ magval[2] = (mdata.z - magbias[2]);
+ mag_x = -magval[0]/magbias[3];
+ mag_y = -magval[1]/magbias[3];
+ mag_z = -magval[2]/magbias[3];
+
+ accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
+ magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z);
+ accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]);
+ magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]);
+ calcMagRef(mag_x/magnorm, mag_y/magnorm, mag_z/magnorm);
+}
+
+void updateBetweenMeasures(){
+ Matrix phi(4,4);
+ phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
+ << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
+ << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt
+ << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
+ qhat = phi*qhat;
+
+ float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
+ qhat *= (1.0f/ qnorm);
+
+ float q0 = qhat.getNumber( 1, 1 );
+ float q1 = qhat.getNumber( 2, 1 );
+ float q2 = qhat.getNumber( 3, 1 );
+ float q3 = qhat.getNumber( 4, 1 );
+
+ Matrix B(4,3);
+ B << q1 << q2 << q3
+ <<-q0 << q3 <<-q2
+ <<-q3 <<-q0 << q1
+ << q2 <<-q1 <<-q0;
+ B *= 0.5f;
+ Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
+
+
+}
+
+void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){
+
+ Matrix u(3,1);
+ Matrix v(3,1);
+
+ u << u1 << u2 <<u3;
+ v << v1 << v2 <<v3;
+
+ float q0 = qhat.getNumber( 1, 1 );
+ float q1 = qhat.getNumber( 2, 1 );
+ float q2 = qhat.getNumber( 3, 1 );
+ float q3 = qhat.getNumber( 4, 1 );
+
+ Matrix A1(3,3);
+ A1 << q0 << q3 << -q2
+ <<-q3 << q0 << q1
+ <<q2 <<-q1 <<q0;
+ A1 *= 2.0f;
+
+ Matrix A2(3,3);
+ A2 << q1 << q2 << q3
+ << q2 <<-q1 << q0
+ << q3 <<-q0 <<-q1;
+ A2 *= 2.0f;
+
+ Matrix A3(3,3);
+ A3 <<-q2 << q1 <<-q0
+ << q1 << q2 << q3
+ << q0 << q3 <<-q2;
+ A3 *= 2.0f;
+
+ Matrix A4(3,3);
+ A4 <<-q3 << q0 << q1
+ <<-q0 <<-q3 << q2
+ << q1 << q2 << q3;
+ A4 *= 2.0f;
+
+ Matrix H(3,4);
+
+ Matrix ab1(A1*u);
+ Matrix ab2(A2*u);
+ Matrix ab3(A3*u);
+ Matrix ab4(A4*u);
+
+ H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
+ << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
+ << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
+
+ D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
+ D.add(1,2,2*(q1*q2 + q0*q3));
+ D.add(1,3,2*(q1*q3 - q0*q2));
+ D.add(2,1,2*(q1*q2 - q0*q3));
+ D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
+ D.add(2,3,2*(q2*q3 + q0*q1));
+ D.add(3,1,2*(q1*q3 + q0*q2));
+ D.add(3,2,2*(q2*q3 - q0*q1));
+ D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
+
+
+ Matrix K(4,3);
+ K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+
+ Matrix dq(4,1);
+ dq = K*(v-D*u);
+ qhat = qhat+dq;
+
+ float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
+ qhat *= (1.0f/ qnorm);
+
+ Matrix eye4(4,4);
+ eye4 << 1 << 0 << 0 << 0
+ << 0 << 1 << 0 << 0
+ << 0 << 0 << 1 << 0
+ << 0 << 0 << 0 << 1;
+ Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
+}
+
+void computeAngles()
{
- pc.printf("gx: %d, gy: %d, gz: %d roll: %f, pitch: %f \r\n",gx,gy,gz,roll* 57.29578f,pitch* 57.29578f);
+ float q0 = qhat.getNumber( 1, 1 );
+ float q1 = qhat.getNumber( 2, 1 );
+ float q2 = qhat.getNumber( 3, 1 );
+ float q3 = qhat.getNumber( 4, 1 );
+ roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
+ pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
+ yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+}
+
+void triad(float fb1,float fb2, float fb3, float fn1, float fn2, float fn3,float mb1,float mb2, float mb3, float mn1, float mn2, float mn3){
+ Matrix W1(3,1);
+ W1 << fb1 << fb2<< fb3;
+ Matrix W2(3,1);
+ W2 << mb1 << mb2<< mb3;
+
+ Matrix V1(3,1);
+ V1 << fn1 << fn2<< fn3;
+ Matrix V2(3,1);
+ V2 << mn1 << mn2<< mn3;
+
+
+ Matrix Ou2(3,1);
+ Ou2 << W1.getNumber( 2, 1 )*W2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*W2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*W2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*W2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*W2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*W2.getNumber( 1, 1 );
+ Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
+ Matrix Ou3(3,1);
+ Ou3 << W1.getNumber( 2, 1 )*Ou2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*Ou2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*Ou2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*Ou2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*Ou2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*Ou2.getNumber( 1, 1 );
+ Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
+ Matrix R2(3,1);
+ R2 << V1.getNumber( 2, 1 )*V2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*V2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*V2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*V2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*V2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*V2.getNumber( 1, 1 );
+ R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
+ Matrix R3(3,1);
+ R3 << V1.getNumber( 2, 1 )*R2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*R2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*R2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*R2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*R2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*R2.getNumber( 1, 1 );
+ R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
+
+ Matrix Mou(3,3);
+ Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 )
+ << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 )
+ << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 );
+ Matrix Mr(3,3);
+ Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 )
+ << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 )
+ << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 );
+
+ Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
+
+ float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 ));
+
+ qhat.add(1,1,0.5*sqtrp1);
+ qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1);
+ qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1);
+ qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1);
+
+ float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
+ qhat *= (1.0f/ qnorm);
+}
+
+void calcDynAcc(){
+ dynacc_x = acc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]);
+ dynacc_y = acc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]);
+ dynacc_z = acc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]);
+}
+
+void execCalibration(){
+ pc.printf("\r\nEnter to Calibration Mode\r\n");
+ wait(5);
+ pc.printf("\r\n Acc and Gyro Calibration Start\r\n");
+ int axs = 0;
+ int ays = 0;
+ int azs = 0;
+ int gxs = 0;
+ int gys = 0;
+ int gzs = 0;
+ int iter_n = 5000;
+ for(int i = 0;i<iter_n ;i++){
+ accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
+ axs += ax;
+ ays += ay;
+ azs -= az;
+ gxs += gx;
+ gys += gy;
+ gzs -= gz;
+ wait(0.001);
+ }
+ axs = axs /iter_n;
+ ays = ays /iter_n;
+ azs = azs /iter_n;
+ gxs = gxs /iter_n;
+ gys = gys /iter_n;
+ gzs = gzs /iter_n;
+ pc.printf("\r\nagoffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
+
+ pc.printf("\r\n Mag Calibration Start\r\n");
+ float f = 0;
+ while(1){
+ mag.getAxis(mdata); // flush the magnetmeter
+ magval[0] = (mdata.x - magbias[0]);
+ magval[1] = (mdata.y - magbias[1]);
+ magval[2] = (mdata.z - magbias[2]);
+ float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
+ float lr = 0.00001f;
+ f = mag_r - magbias[3]*magbias[3];
+ magbias[0] = magbias[0] + 4 * lr * f * magval[0];
+ magbias[1] = magbias[1] + 4 * lr * f * magval[1];
+ magbias[2] = magbias[2] + 4 * lr * f * magval[2];
+ magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
+
+ if(userButton.read() == 1){
+ break;
+ }
+ wait(0.001);
+ }
+ pc.printf("magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]);
+ pc.printf("\r\n Refvec Calibration waiting\r\n");
+ wait(5);
+ pc.printf("\r\n Calibration Start\r\n");
+ float arefs[3] = {0.0,0.0,0.0};
+ float mrefs[3] = {0.0,0.0,0.0};
+ for(int i = 0;i<iter_n ;i++){
+ getIMUval();
+ arefs[0] += acc_x;
+ arefs[1] += acc_y;
+ arefs[2] += acc_z;
+ mrefs[0] += mag_x;
+ mrefs[1] += mag_y;
+ mrefs[2] += mag_z;
+ wait(0.001);
+ }
+ arefs[0] /= iter_n;
+ arefs[1] /= iter_n;
+ arefs[2] /= iter_n;
+ mrefs[0] /= iter_n;
+ mrefs[1] /= iter_n;
+ mrefs[2] /= iter_n;
+ pc.printf("\r\naccreg : %f, %f, %f\r\n",arefs[0],arefs[1],arefs[2]);
+ pc.printf("\r\nmagreg : %f, %f, %f\r\n",mrefs[0],mrefs[1],mrefs[2]);
+ while(1) {wait(1000);}
}
// 割り込まれた時点での出力(computeの結果）を返す関数
@@ -88,14 +400,11 @@
}
}
-
- //pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
- //roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
pitchPID.setProcessValue(pitch);
pitchratePID.setProcessValue(gyro_y);
rollPID.setProcessValue(roll);
rollratePID.setProcessValue(gyro_x);
- float de = -pitchPID.compute()-pitchratePID.compute()-rc[1]+rc[0];
+ float de = pitchPID.compute()+pitchratePID.compute()-rc[1]+rc[0];
float da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1];
float dT = rc[2];
@@ -103,8 +412,8 @@
scaledServoOut[1]=-de+da;
scaledMotorOut[0]= dT;
- double LP_servo = 0.2;
- double LP_motor = 0.2;
+ float LP_servo = 0.2;
+ float LP_motor = 0.2;
for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
if(servoOut[i]<servoPwmMin) {
@@ -136,156 +445,6 @@
}
}
-void updateAttitude(){
- // gx gy gz ax ay az
- accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
- // 加速度値を分解能で割って加速度(G)に変換する
- acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
- acc_y = ay / ACCEL_SSF;
- acc_z = az / ACCEL_SSF;
- // 角速度値を分解能で割って角速度(deg per sec)に変換する
- gyro_x = gx / GYRO_SSF; // (deg/s)
- gyro_y = gy / GYRO_SSF;
- gyro_z = gz / GYRO_SSF;
- //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
- //MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
-}
-
-void getIMUval(){
- // gx gy gz ax ay az
- accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
- ax = ax - offsetVal[0];
- ay = ay - offsetVal[1];
- az = az - offsetVal[2];
- gx = gx - offsetVal[3];
- gy = gy - offsetVal[4];
- gz = gz - offsetVal[5];
- // 加速度値を分解能で割って加速度(G)に変換する
- acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
- acc_y = float(ay) / ACCEL_SSF;
- acc_z = float(az) / ACCEL_SSF;
- // 角速度値を分解能で割って角速度(rad per sec)に変換する
- gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
- gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
- gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
-}
-
-void updateBetweenMeasures(){
-
- getIMUval();
-
- Matrix phi(4,4);
- phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
- << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
- << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt
- << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
- qhat = phi*qhat;
-
- float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
- qhat *= (1.0f/ qnorm);
-
- float q0 = qhat.getNumber( 1, 1 );
- float q1 = qhat.getNumber( 2, 1 );
- float q2 = qhat.getNumber( 3, 1 );
- float q3 = qhat.getNumber( 4, 1 );
-
- Matrix B(4,3);
- B << q1 << q2 << q3
- <<-q0 << q3 <<-q2
- <<-q3 <<-q0 << q1
- << q2 <<-q1 <<-q0;
- B *= 0.5f;
- Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
-
-
-}
-
-void updateAcrossMeasures(){
- getIMUval();
-
- Matrix uacc(3,1);
- Matrix vacc(3,1);
-
- uacc << 0 << 0 << -1;
- float accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
- vacc << acc_x/accnorm << acc_y/accnorm << acc_z/accnorm;
-
- float q0 = qhat.getNumber( 1, 1 );
- float q1 = qhat.getNumber( 2, 1 );
- float q2 = qhat.getNumber( 3, 1 );
- float q3 = qhat.getNumber( 4, 1 );
-
- Matrix A1(3,3);
- A1 << q0 << q3 << -q2
- <<-q3 << q0 << q1
- <<q2 <<-q1 <<q0;
- A1 *= 2.0f;
-
- Matrix A2(3,3);
- A2 << q1 << q2 << q3
- << q2 <<-q1 << q0
- << q3 <<-q0 <<-q1;
- A2 *= 2.0f;
-
- Matrix A3(3,3);
- A3 <<-q2 << q1 <<-q0
- << q1 << q2 << q3
- << q0 << q3 <<-q2;
- A3 *= 2.0f;
-
- Matrix A4(3,3);
- A4 <<-q3 << q0 << q1
- <<-q0 <<-q3 << q2
- << q1 << q2 << q3;
- A4 *= 2.0f;
-
- Matrix H(3,4);
-
- Matrix ab1(A1*uacc);
- Matrix ab2(A2*uacc);
- Matrix ab3(A3*uacc);
- Matrix ab4(A4*uacc);
-
- H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
- << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
- << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
-
- Matrix D(3,3);
- D << q0*q0 + q1*q1 - q2*q2 - q3*q3
- << 2*(q1*q2 + q0*q3)
- << 2*(q1*q3 - q0*q2)
- << 2*(q1*q2 - q0*q3)
- << q0*q0 - q1*q1 + q2*q2 - q3*q3
- << 2*(q2*q3 + q0*q1)
- << 2*(q1*q3 + q0*q2)
- << 2*(q2*q3 - q0*q1)
- << q0*q0 - q1*q1 - q2*q2 + q3*q3;
-
- Matrix err(3,1);
- err = vacc-D*uacc;
-
- Matrix K(4,3);
- K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Racc);
-
- Matrix dq(4,1);
- dq = K*err;
- qhat = qhat-dq;
-
- float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
- qhat *= (1.0f/ qnorm);
-}
-
-void computeAngles()
-{
- float q0 = qhat.getNumber( 1, 1 );
- float q1 = qhat.getNumber( 2, 1 );
- float q2 = qhat.getNumber( 3, 1 );
- float q3 = qhat.getNumber( 4, 1 );
- roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
- pitch = asinf(-2.0f * (q1*q3 - q0*q2));
- yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
-}
-
int main()
{
pitchPID.setSetPoint(0.0);
@@ -312,48 +471,75 @@
servoLeft.pulsewidth_us(1500.0);
servoThrust.pulsewidth_us(1100.0);
- qhat << 1 << 0 << 0 << 0;
-
- Phat << 0.0001 << 0 <<0 <<0
- << 0 << 0.0001 <<0 <<0
- << 0 << 0 <<0.0001 <<0
- << 0 << 0 << 0 << 0.0001;
-
- Qgyro << 0.0001 << 0 <<0
- << 0 << 0.0001 <<0
- << 0 << 0 <<0.0001;
-
- Racc << 0.0001 << 0 <<0
- << 0 << 0.0001 <<0
- << 0 << 0 <<0.0001;
-
-
- LoopTicker PIDtick;
- PIDtick.attach(calcServoOut,PID_dt);
- pc.baud(115200);
- //sd.baud(115200);
+ pc.baud(57600);
+ sd.baud(57600);
+ sd.printf("\r\n Program Start \r\n");
accelgyro.initialize();
//加速度計のフルスケールレンジを設定
accelgyro.setFullScaleAccelRange(ACCEL_FSR);
- //■角速度計のフルスケールレンジを設定
+ //角速度計のフルスケールレンジを設定
accelgyro.setFullScaleGyroRange(GYRO_FSR);
//MPU6050のLPFを設定
accelgyro.setDLPFMode(MPU6050_LPF);
- //MadgwickFilter.begin(100); //サンプリング周波数Hza
- t.start();
- //servoRight.write(0.0f);
- while(1) {
- float tstart = t.read();
- //姿勢角を更新
- //updateAttitude();
- updateBetweenMeasures();
- updateAcrossMeasures();
- computeAngles();
- PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
+ //地磁気
+ mag.enable();
+
+ if(userButton.read() == 0){
+ qhat << 1 << 0 << 0 << 0;
+
+ Phat << 0.01 << 0 <<0 <<0
+ << 0 << 0.01 <<0 <<0
+ << 0 << 0 <<0.001 <<0
+ << 0 << 0 << 0 << 0.001;
+
+ Qgyro << 5.4732e-04 << 0 <<0
+ << 0 <<5.4732e-04 <<0
+ << 0 << 0 <<5.4732e-04;
- float tend = t.read();
- att_dt = (tend-tstart);
- //MadgwickFilter.begin(1.0f/att_dt); //サンプリング周波数Hz
+ Racc.add(1,1,1.0);
+ Racc.add(2,2,1.0);
+ Racc.add(3,3,1.0);
+
+ Rmag << 1 << 0 <<0
+ << 0 << 1 <<0
+ << 0 << 0 <<1;
+ getIMUval();
+ triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm);
+ for(int i = 0; i<1000 ;i++){
+ getIMUval();
+ val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm);
+ }
+ val_thmg /= 1000;
+ LoopTicker PIDtick;
+ PIDtick.attach(calcServoOut,PID_dt);
+
+ t.start();
+ while(1) {
+ float tstart = t.read();
+ //姿勢角を更新
+ getIMUval();
+ updateBetweenMeasures();
+ calcDynAcc();
+ float th_mg = acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm);
+ float dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z);
+
+ //pc.printf("%f %f : %f \r\n",th_mg,th_mgref,abs(th_mg-th_mgref));
+ int ang_th = abs(th_mg-val_thmg)<0.005;
+ int dyn_th = dynaccnorm < 0.01/4;
+ int norm_th = abs(accnorm-accrefnorm)< 0.01/6.0;
+ //pc.printf("%d %d %d %f %f %f\r\n",ang_th,dyn_th,norm_th,abs(th_mg-val_thmg),dynaccnorm ,abs(accnorm-accrefnorm));
+ if(ang_th+dyn_th+norm_th>1){
+ updateAcrossMeasures(mag_x/magnorm, mag_y/magnorm, mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
+ updateAcrossMeasures(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
+ }
+ computeAngles();
+ PIDtick.loop();
+
+ float tend = t.read();
+ att_dt = (tend-tstart);
+ }
+ }else{
+ execCalibration();
}
}
\ No newline at end of file