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@140:53dbdb207542, 2021-12-06 (annotated)

Committer:
cocorlow
Date:
Mon Dec 06 11:37:55 2021 +0000
Revision:
140:53dbdb207542
Parent:
139:b378528c05f2
Child:
141:725321fe2949
servo transferData imu

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 140:53dbdb207542 8 rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
cocorlow 140:53dbdb207542 9 }
cocorlow 140:53dbdb207542 10
cocorlow 140:53dbdb207542 11
cocorlow 140:53dbdb207542 12 //姿勢角の所得
cocorlow 140:53dbdb207542 13 euler = eskf.computeAngles();
cocorlow 140:53dbdb207542 14 rpy = euler;
cocorlow 140:53dbdb207542 15
cocorlow 140:53dbdb207542 16 //PIDへの状態量のセット
cocorlow 140:53dbdb207542 17 pitchPID.setProcessValue(rpy(1));
cocorlow 140:53dbdb207542 18 pitchratePID.setProcessValue(gyro(1));
cocorlow 140:53dbdb207542 19 rollPID.setProcessValue(rpy(0));
cocorlow 140:53dbdb207542 20 rollratePID.setProcessValue(gyro(0));
cocorlow 140:53dbdb207542 21
cocorlow 140:53dbdb207542 22 dT = rc[2];
cocorlow 140:53dbdb207542 23
cocorlow 140:53dbdb207542 24 if (rc[4]>-0.3f && rc[6] < -0.3f)
cocorlow 140:53dbdb207542 25 {
cocorlow 140:53dbdb207542 26 //level_flight();
cocorlow 140:53dbdb207542 27 //point_guide();
cocorlow 140:53dbdb207542 28 climb();
cocorlow 140:53dbdb207542 29 rollPID.setSetPoint(roll_obj);
cocorlow 140:53dbdb207542 30 pitchPID.setSetPoint(pitch_obj);
cocorlow 140:53dbdb207542 31 dT += dT_obj;
cocorlow 140:53dbdb207542 32 }else{
cocorlow 140:53dbdb207542 33 rollPID.setSetPoint(0.0f);
cocorlow 140:53dbdb207542 34 pitchPID.setSetPoint(0.0f);
cocorlow 140:53dbdb207542 35 }
cocorlow 140:53dbdb207542 36
cocorlow 140:53dbdb207542 37 //舵角計算
cocorlow 140:53dbdb207542 38 if(rc[4]<-0.3f){
cocorlow 140:53dbdb207542 39 de = (rc[0]-rc[1])/2.0f;
cocorlow 140:53dbdb207542 40 da = (rc[0]+rc[1])/2.0f;
cocorlow 140:53dbdb207542 41 }else{
cocorlow 140:53dbdb207542 42 de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
cocorlow 140:53dbdb207542 43 da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
cocorlow 140:53dbdb207542 44 }
cocorlow 140:53dbdb207542 45
cocorlow 140:53dbdb207542 46 scaledServoOut[0]=de+da;
cocorlow 140:53dbdb207542 47 scaledServoOut[1]=-de+da;
cocorlow 140:53dbdb207542 48 scaledMotorOut[0]= dT;
cocorlow 140:53dbdb207542 49
cocorlow 140:53dbdb207542 50 float LP_servo = 0.2;
cocorlow 140:53dbdb207542 51 float LP_motor = 0.2;
cocorlow 140:53dbdb207542 52 for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
cocorlow 140:53dbdb207542 53 {
cocorlow 140:53dbdb207542 54 servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1.0f,1.0f,servoPwmMin,servoPwmMax))+(1.0f-LP_servo)*servoOut[i];
cocorlow 140:53dbdb207542 55 if(servoOut[i]<servoPwmMin)
cocorlow 140:53dbdb207542 56 {
cocorlow 140:53dbdb207542 57 servoOut[i] = servoPwmMin;
cocorlow 140:53dbdb207542 58 }
cocorlow 140:53dbdb207542 59 if(servoOut[i]>servoPwmMax)
cocorlow 140:53dbdb207542 60 {
cocorlow 140:53dbdb207542 61 servoOut[i] = servoPwmMax;
cocorlow 140:53dbdb207542 62 }
cocorlow 140:53dbdb207542 63 }
cocorlow 140:53dbdb207542 64
cocorlow 140:53dbdb207542 65 for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
cocorlow 140:53dbdb207542 66 motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1.0f,1.0f,motorPwmMin,motorPwmMax))+(1.0f-LP_motor)*motorOut[i];
cocorlow 140:53dbdb207542 67 if(motorOut[i]<motorPwmMin) {
cocorlow 140:53dbdb207542 68 motorOut[i] = motorPwmMin;
cocorlow 140:53dbdb207542 69 };
cocorlow 140:53dbdb207542 70 if(motorOut[i]>motorPwmMax) {
cocorlow 140:53dbdb207542 71 motorOut[i] = motorPwmMax;
cocorlow 140:53dbdb207542 72 };
cocorlow 140:53dbdb207542 73 }
cocorlow 140:53dbdb207542 74 servoRight.pulsewidth_us(servoOut[0]);
cocorlow 140:53dbdb207542 75 servoLeft.pulsewidth_us(servoOut[1]);
cocorlow 140:53dbdb207542 76 servoThrust.pulsewidth_us(motorOut[0]);
cocorlow 140:53dbdb207542 77
cocorlow 140:53dbdb207542 78 sendData2PC();
cocorlow 140:53dbdb207542 79 writeSDcard();
cocorlow 140:53dbdb207542 80
cocorlow 140:53dbdb207542 81 if(loop_count >= 5)
cocorlow 140:53dbdb207542 82 {
cocorlow 140:53dbdb207542 83 sendTelemetry();
cocorlow 140:53dbdb207542 84 loop_count = 1;
cocorlow 140:53dbdb207542 85
cocorlow 140:53dbdb207542 86 }
cocorlow 140:53dbdb207542 87 else
cocorlow 140:53dbdb207542 88 {
cocorlow 140:53dbdb207542 89 loop_count +=1;
cocorlow 140:53dbdb207542 90 }
cocorlow 140:53dbdb207542 91 }