HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

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

servo.cpp@66:e5afad70fdd8, 2021-06-22 (annotated)

Committer:
NaotoMorita
Date:
Tue Jun 22 02:19:14 2021 +0000
Revision:
66:e5afad70fdd8
Parent:
62:ef10fd919f7b
Child:
68:b9f6938fab9d
mbed ram start;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 56:888379912f81 1 #include "global.hpp"
cocorlow 56:888379912f81 2
cocorlow 56:888379912f81 3 // 割り込まれた時点での出力(computeの結果）を返す関数
cocorlow 56:888379912f81 4 void calcServoOut()
cocorlow 56:888379912f81 5 {
NaotoMorita 57:923e3df16159 6
NaotoMorita 57:923e3df16159 7 // 自身の位置に応じてエレベータ舵角を決定する
NaotoMorita 57:923e3df16159 8 float deobj = 0.0f;
NaotoMorita 66:e5afad70fdd8 9 float drobj = 0.0f;
NaotoMorita 66:e5afad70fdd8 10
NaotoMorita 57:923e3df16159 11 //指令角の最大最小値をrc7で変更
NaotoMorita 66:e5afad70fdd8 12 float pitchobj = 10.0f*M_PI/180.0f * deobj;
NaotoMorita 57:923e3df16159 13
NaotoMorita 57:923e3df16159 14 //ゲインの係数をrc8で変更
NaotoMorita 62:ef10fd919f7b 15 float gaincoef = 1.0;
NaotoMorita 62:ef10fd919f7b 16 pitchPID.setGain(6.36*gaincoef, 10.6*gaincoef*0.0f,0.0);
NaotoMorita 57:923e3df16159 17 pitchratePID.setGain(0.9540*gaincoef,0.0,0.0);
cocorlow 56:888379912f81 18 pitchPID.setProcessValue(rpy.y);
cocorlow 56:888379912f81 19 pitchratePID.setProcessValue(gyro.y);
NaotoMorita 57:923e3df16159 20 //目標値のセット
NaotoMorita 57:923e3df16159 21 pitchPID.setSetPoint(pitchobj); //目標の設定
NaotoMorita 57:923e3df16159 22 float de = pitchPID.compute()+pitchratePID.compute();
NaotoMorita 66:e5afad70fdd8 23 float dr = 0.0f;
cocorlow 56:888379912f81 24 scaledServoOut[0]=de;
NaotoMorita 66:e5afad70fdd8 25 scaledServoOut[1]=dr;
cocorlow 56:888379912f81 26 float LP_servo = 0.2;
cocorlow 56:888379912f81 27 for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
cocorlow 56:888379912f81 28 {
cocorlow 56:888379912f81 29 servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
cocorlow 56:888379912f81 30 if(servoOut[i]<servoPwmMin)
cocorlow 56:888379912f81 31 {
cocorlow 56:888379912f81 32 servoOut[i] = servoPwmMin;
cocorlow 56:888379912f81 33 }
cocorlow 56:888379912f81 34 if(servoOut[i]>servoPwmMax)
cocorlow 56:888379912f81 35 {
cocorlow 56:888379912f81 36 servoOut[i] = servoPwmMax;
cocorlow 56:888379912f81 37 }
cocorlow 56:888379912f81 38 }
cocorlow 56:888379912f81 39
NaotoMorita 66:e5afad70fdd8 40 elevServo.pulsewidth_us(servoOut[0]);
NaotoMorita 66:e5afad70fdd8 41 rudServo.pulsewidth_us(servoOut[1]);
cocorlow 56:888379912f81 42
cocorlow 56:888379912f81 43 if(loop_count >= 10)
cocorlow 56:888379912f81 44 {
cocorlow 56:888379912f81 45 writeSdcard();
cocorlow 56:888379912f81 46 loop_count = 1;
cocorlow 56:888379912f81 47 }
cocorlow 56:888379912f81 48 else
cocorlow 56:888379912f81 49 {
cocorlow 56:888379912f81 50 loop_count +=1;
cocorlow 56:888379912f81 51 }
cocorlow 56:888379912f81 52 }