HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

servo.cpp@144:b3a713b4f1c4, 22 months ago (annotated)

Committer:
NaotoMorita
Date:
Wed Jun 29 07:57:10 2022 +0000
Revision:
144:b3a713b4f1c4
Parent:
143:53808e4e684c 
can fly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 140:53dbdb207542 1 #include "global.hpp"
cocorlow 140:53dbdb207542 2
cocorlow 140:53dbdb207542 3 // 割り込まれた時点での出力(computeの結果）を返す関数
cocorlow 140:53dbdb207542 4 void calcServoOut()
cocorlow 140:53dbdb207542 5 {
cocorlow 140:53dbdb207542 6 // sbusデータの読み込み
cocorlow 140:53dbdb207542 7 for (int i =0 ; i < 16;i ++){
cocorlow 141:725321fe2949 8 rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input
cocorlow 140:53dbdb207542 9 }
cocorlow 140:53dbdb207542 10
cocorlow 140:53dbdb207542 11 //姿勢角の所得
cocorlow 140:53dbdb207542 12 euler = eskf.computeAngles();
NaotoMorita 144:b3a713b4f1c4 13 euler(1) -= 3.0f*M_PI_F/180.0f;
NaotoMorita 143:53808e4e684c 14 Vector3f gyroBias = eskf.getGyroBias();
cocorlow 140:53dbdb207542 15 //PIDへの状態量のセット
NaotoMorita 143:53808e4e684c 16 pitchPID.setProcessValue(euler(1));
NaotoMorita 143:53808e4e684c 17 pitchratePID.setProcessValue(gyro(1)-gyroBias(1));
NaotoMorita 143:53808e4e684c 18 rollPID.setProcessValue(euler(0));
NaotoMorita 143:53808e4e684c 19 rollratePID.setProcessValue(gyro(0)-gyroBias(0));
NaotoMorita 143:53808e4e684c 20 yawratePID.setProcessValue(gyro(2)-gyroBias(2));
NaotoMorita 144:b3a713b4f1c4 21
NaotoMorita 144:b3a713b4f1c4 22 vxPID.setGain(0.005f,0.0f,0.0f);
NaotoMorita 144:b3a713b4f1c4 23 vyPID.setGain(0.005f,0.0f,0.0f);
NaotoMorita 144:b3a713b4f1c4 24
NaotoMorita 144:b3a713b4f1c4 25
NaotoMorita 144:b3a713b4f1c4 26 pitchPID.setGain(20.0f,0.0f,0.0f);
NaotoMorita 144:b3a713b4f1c4 27 pitchratePID.setGain(0.18f,0.0f,0.0f);
NaotoMorita 144:b3a713b4f1c4 28 rollPID.setGain(20.0f,0.0f,0.0f);
NaotoMorita 144:b3a713b4f1c4 29 rollratePID.setGain(0.18f,0.0f,0.0f);
cocorlow 140:53dbdb207542 30
NaotoMorita 144:b3a713b4f1c4 31 Vector3f Vb = eskf.calcVb();
NaotoMorita 144:b3a713b4f1c4 32 Vector3f dynAcc = eskf.calcDynAcc(acc);
NaotoMorita 144:b3a713b4f1c4 33 vxPID.setProcessValue(dynAcc(0));
NaotoMorita 144:b3a713b4f1c4 34 vxPID.setSetPoint(0.0f);
NaotoMorita 144:b3a713b4f1c4 35 vyPID.setProcessValue(dynAcc(1));
NaotoMorita 144:b3a713b4f1c4 36 vyPID.setSetPoint(0.0f);
NaotoMorita 144:b3a713b4f1c4 37 vzPID.setProcessValue(Vb(2));
NaotoMorita 144:b3a713b4f1c4 38 vzPID.setSetPoint(0.0f);
NaotoMorita 144:b3a713b4f1c4 39
NaotoMorita 144:b3a713b4f1c4 40 //dT = -vzPID.compute()+rc[2];
NaotoMorita 144:b3a713b4f1c4 41 dT = +rc[2];
cocorlow 140:53dbdb207542 42
NaotoMorita 144:b3a713b4f1c4 43 float pitchobj = rc[1]* 30.0f*M_PI_F/180.0f;
NaotoMorita 144:b3a713b4f1c4 44 float rollobj = rc[0] * 30.0f*M_PI_F/180.0f;
NaotoMorita 144:b3a713b4f1c4 45 yawratePID.setSetPoint(0.0f);
NaotoMorita 144:b3a713b4f1c4 46 rollPID.setSetPoint(rollobj);
NaotoMorita 144:b3a713b4f1c4 47 pitchPID.setSetPoint(pitchobj);
NaotoMorita 144:b3a713b4f1c4 48
NaotoMorita 144:b3a713b4f1c4 49 float pitchrateobj = pitchPID.compute()* 30.0f*M_PI_F/180.0f;
NaotoMorita 144:b3a713b4f1c4 50 float rollrateobj = rollPID.compute()* 30.0f*M_PI_F/180.0f;
NaotoMorita 144:b3a713b4f1c4 51 pitchratePID.setSetPoint(pitchrateobj);
NaotoMorita 144:b3a713b4f1c4 52 rollratePID.setSetPoint(rollrateobj);
NaotoMorita 144:b3a713b4f1c4 53
NaotoMorita 144:b3a713b4f1c4 54 de = pitchratePID.compute();
NaotoMorita 144:b3a713b4f1c4 55 da = rollratePID.compute();
NaotoMorita 144:b3a713b4f1c4 56 dr = -0.1f*rc[3]+yawratePID.compute();
NaotoMorita 144:b3a713b4f1c4 57
NaotoMorita 144:b3a713b4f1c4 58 scaledMotorOut[0]= dT+de-dr;
NaotoMorita 144:b3a713b4f1c4 59 scaledMotorOut[1]= dT+0.866f*de-0.866f*da+dr;
NaotoMorita 144:b3a713b4f1c4 60 scaledMotorOut[2]= dT-0.866f*de-0.866f*da-dr;
NaotoMorita 144:b3a713b4f1c4 61 scaledMotorOut[3]= dT-de+dr;
NaotoMorita 144:b3a713b4f1c4 62 scaledMotorOut[4]= dT-0.866f*de+0.866f*da-dr;
NaotoMorita 144:b3a713b4f1c4 63 scaledMotorOut[5]= dT+0.866f*de+0.866f*da+dr;
NaotoMorita 144:b3a713b4f1c4 64
NaotoMorita 144:b3a713b4f1c4 65 float minArmThrottle = 0.25f;
NaotoMorita 144:b3a713b4f1c4 66 for(int i = 0;i<sizeof(scaledMotorOut)/sizeof(scaledMotorOut[0]) ;i++){
NaotoMorita 144:b3a713b4f1c4 67 if(scaledMotorOut[i]<-1.0f+minArmThrottle) {
NaotoMorita 144:b3a713b4f1c4 68 scaledMotorOut[i] = -1.0f+minArmThrottle;
NaotoMorita 144:b3a713b4f1c4 69 }
NaotoMorita 143:53808e4e684c 70 }
NaotoMorita 143:53808e4e684c 71
NaotoMorita 144:b3a713b4f1c4 72 //pc.serial.printf("%f %f %f %f\r\n",dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
cocorlow 140:53dbdb207542 73
NaotoMorita 144:b3a713b4f1c4 74 float LP_motor = 1.0f;
NaotoMorita 144:b3a713b4f1c4 75 for(int i = 0;i<6 ;i++){
NaotoMorita 144:b3a713b4f1c4 76 motorOut[i] = (mapfloat(scaledMotorOut[i],-1.0f,1.0f,motorPwmMin,motorPwmMax));
cocorlow 140:53dbdb207542 77 if(motorOut[i]<motorPwmMin) {
cocorlow 140:53dbdb207542 78 motorOut[i] = motorPwmMin;
cocorlow 140:53dbdb207542 79 };
cocorlow 140:53dbdb207542 80 if(motorOut[i]>motorPwmMax) {
cocorlow 140:53dbdb207542 81 motorOut[i] = motorPwmMax;
cocorlow 140:53dbdb207542 82 };
cocorlow 140:53dbdb207542 83 }
NaotoMorita 144:b3a713b4f1c4 84
NaotoMorita 144:b3a713b4f1c4 85 if(rc[7]>0.3f){
NaotoMorita 144:b3a713b4f1c4 86 motor1.pulsewidth_us(motorOut[0]);
NaotoMorita 144:b3a713b4f1c4 87 motor2.pulsewidth_us(motorOut[1]);
NaotoMorita 144:b3a713b4f1c4 88 motor3.pulsewidth_us(motorOut[2]);
NaotoMorita 144:b3a713b4f1c4 89 motor4.pulsewidth_us(motorOut[3]);
NaotoMorita 144:b3a713b4f1c4 90 motor5.pulsewidth_us(motorOut[4]);
NaotoMorita 144:b3a713b4f1c4 91 motor6.pulsewidth_us(motorOut[5]);
NaotoMorita 144:b3a713b4f1c4 92 }else{
NaotoMorita 144:b3a713b4f1c4 93 motor1.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 94 motor2.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 95 motor3.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 96 motor4.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 97 motor5.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 98 motor6.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 99 }
cocorlow 140:53dbdb207542 100 sendData2PC();
cocorlow 140:53dbdb207542 101 writeSDcard();
cocorlow 140:53dbdb207542 102
cocorlow 140:53dbdb207542 103 if(loop_count >= 5)
cocorlow 140:53dbdb207542 104 {
cocorlow 140:53dbdb207542 105 sendTelemetry();
cocorlow 140:53dbdb207542 106 loop_count = 1;
cocorlow 140:53dbdb207542 107
cocorlow 140:53dbdb207542 108 }
cocorlow 140:53dbdb207542 109 else
cocorlow 140:53dbdb207542 110 {
cocorlow 140:53dbdb207542 111 loop_count +=1;
cocorlow 140:53dbdb207542 112 }
cocorlow 140:53dbdb207542 113 }