solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
global.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?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 56:888379912f81 | 1 | #include "global.hpp" |
cocorlow | 56:888379912f81 | 2 | |
NaotoMorita | 67:41fcdfb7cc5a | 3 | |
cocorlow | 56:888379912f81 | 4 | // var |
cocorlow | 56:888379912f81 | 5 | |
cocorlow | 56:888379912f81 | 6 | // communication |
NaotoMorita | 82:c183c29d2427 | 7 | UsaPack pc(USBTX, USBRX, 115200); // log - tail |
NaotoMorita | 132:896ad37b534b | 8 | UsaPack sd(PG_14,PG_9, 115200); |
NaotoMorita | 134:d57c6b2a706b | 9 | UsaPack twelite(PD_1,PD_0,38400); |
cocorlow | 56:888379912f81 | 10 | // io |
cocorlow | 56:888379912f81 | 11 | DigitalIn userButton(USER_BUTTON); |
NaotoMorita | 70:99f974d8960e | 12 | SBUS sbus(PD_5, PD_6); |
osaka | 88:be349faa1976 | 13 | I2C i2c(PB_9, PB_8); |
osaka | 88:be349faa1976 | 14 | LSM9DS1 lsm(i2c); |
osaka | 88:be349faa1976 | 15 | LPS lps(i2c); |
NaotoMorita | 99:98b892ca70ec | 16 | GPSUBX_UART gps(PF_7, PE_7); |
NaotoMorita | 119:a21e283730d1 | 17 | CalibrateMagneto magCalibrator; |
NaotoMorita | 102:1c77ff6e2a85 | 18 | float magres = 0.0f; |
cocorlow | 56:888379912f81 | 19 | // control |
NaotoMorita | 94:579e875a4244 | 20 | Timer _t; |
NaotoMorita | 144:b3a713b4f1c4 | 21 | FastPWM motor1(PE_9); |
NaotoMorita | 144:b3a713b4f1c4 | 22 | FastPWM motor2(PE_11); |
NaotoMorita | 144:b3a713b4f1c4 | 23 | FastPWM motor3(PE_13); |
NaotoMorita | 144:b3a713b4f1c4 | 24 | FastPWM motor4(PA_6); |
NaotoMorita | 144:b3a713b4f1c4 | 25 | FastPWM motor5(PA_0); |
NaotoMorita | 144:b3a713b4f1c4 | 26 | FastPWM motor6(PB_10); |
NaotoMorita | 144:b3a713b4f1c4 | 27 | PID pitchPID(0.05f,0.0f,0.0f,PID_dt); //rad |
NaotoMorita | 144:b3a713b4f1c4 | 28 | PID pitchratePID(0.05f, 0.0f, 0.0f, PID_dt);//rad/s |
NaotoMorita | 144:b3a713b4f1c4 | 29 | PID rollPID(0.05f,0.0f,0.0f,PID_dt); |
NaotoMorita | 94:579e875a4244 | 30 | PID rollratePID(0.05f, 0.0, 0.0, PID_dt);//rad/s |
NaotoMorita | 144:b3a713b4f1c4 | 31 | PID yawratePID(0.3f, 0.0, 0.0, PID_dt);//rad/s |
NaotoMorita | 144:b3a713b4f1c4 | 32 | PID vxPID(0.5f, 0.1f, 0.0f, PID_dt); |
NaotoMorita | 144:b3a713b4f1c4 | 33 | PID vyPID(0.5f, 0.1f, 0.0f, PID_dt); |
NaotoMorita | 144:b3a713b4f1c4 | 34 | PID vzPID(0.05f, 0.0f, 0.0f, PID_dt); |
cocorlow | 140:53dbdb207542 | 35 | solaESKF eskf; // ESKF class |
cocorlow | 56:888379912f81 | 36 | |
NaotoMorita | 70:99f974d8960e | 37 | float rc[16]; |
cocorlow | 56:888379912f81 | 38 | int loop_count = 0; |
cocorlow | 56:888379912f81 | 39 | float att_dt = 0.01f; |
cocorlow | 56:888379912f81 | 40 | // position |
NaotoMorita | 144:b3a713b4f1c4 | 41 | Matrix3f SensorAlignmentAG = Matrix3f::Zero(); |
NaotoMorita | 144:b3a713b4f1c4 | 42 | Matrix3f SensorAlignmentMAG = Matrix3f::Zero(); |
NaotoMorita | 144:b3a713b4f1c4 | 43 | Vector3f euler = Vector3f::Zero(); |
NaotoMorita | 144:b3a713b4f1c4 | 44 | Vector3f acc = Vector3f::Zero(); |
NaotoMorita | 144:b3a713b4f1c4 | 45 | Vector3f mag = Vector3f::Zero(); |
NaotoMorita | 144:b3a713b4f1c4 | 46 | Vector3f magref = Vector3f::Zero(); |
NaotoMorita | 144:b3a713b4f1c4 | 47 | Vector3f gyro = Vector3f::Zero(); |
NaotoMorita | 144:b3a713b4f1c4 | 48 | Vector3f vi = Vector3f::Zero(); |
NaotoMorita | 144:b3a713b4f1c4 | 49 | Vector3f pi = Vector3f::Zero(); |
NaotoMorita | 92:00460f6df439 | 50 | float palt; |
NaotoMorita | 98:bdaa6bbfb1d9 | 51 | float palt0; |
NaotoMorita | 129:a76be8380bb2 | 52 | float dynaccnorm2; |
NaotoMorita | 106:2d854e92cebb | 53 | int itow_status = 0; |
NaotoMorita | 100:7589b663d462 | 54 | bool gpsUpdateFlag = false; |
NaotoMorita | 106:2d854e92cebb | 55 | bool gpsLlh0Fixed = false; |
NaotoMorita | 127:d73a6233ee4b | 56 | bool headingUpdateFlag = false; |
NaotoMorita | 143:53808e4e684c | 57 | bool hinf_flag = false; |
NaotoMorita | 77:2bf856e3eca4 | 58 | |
NaotoMorita | 73:84ffa0166e6c | 59 | float de = 0.0f; |
NaotoMorita | 73:84ffa0166e6c | 60 | float da = 0.0f; |
NaotoMorita | 144:b3a713b4f1c4 | 61 | float dr = 0.0f; |
NaotoMorita | 73:84ffa0166e6c | 62 | float dT = 0.0f; |
NaotoMorita | 72:267e2cfddb0b | 63 | |
NaotoMorita | 144:b3a713b4f1c4 | 64 | float scaledMotorOut[6]= {-1.0f,-1.0f,-1.0f,-1.0f,-1.0f,-1.0f}; |
NaotoMorita | 144:b3a713b4f1c4 | 65 | float motorOut[6] = {1100.0f,1100.0f,1100.0f,1100.0f,1100.0f,1100.0f}; |
NaotoMorita | 93:b827f78a717a | 66 | float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; |
NaotoMorita | 76:7fd3ac1afe3e | 67 | |
cocorlow | 56:888379912f81 | 68 | |
cocorlow | 56:888379912f81 | 69 | // UsaPack |
NaotoMorita | 73:84ffa0166e6c | 70 | valuePack vp; |
NaotoMorita | 83:e69ab831031c | 71 | sendPack sp; |
NaotoMorita | 132:896ad37b534b | 72 | logPack lp; |
NaotoMorita | 134:d57c6b2a706b | 73 | telemetryPack tp; |
NaotoMorita | 73:84ffa0166e6c | 74 | |
NaotoMorita | 73:84ffa0166e6c | 75 | // HIL |
NaotoMorita | 144:b3a713b4f1c4 | 76 | bool hilFlag = false; |
NaotoMorita | 143:53808e4e684c | 77 | int16_t hilDataOut = 0; |
cocorlow | 56:888379912f81 | 78 | |
cocorlow | 56:888379912f81 | 79 | float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) |
cocorlow | 56:888379912f81 | 80 | { |
cocorlow | 56:888379912f81 | 81 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
NaotoMorita | 67:41fcdfb7cc5a | 82 | } |
NaotoMorita | 93:b827f78a717a | 83 | |
cocorlow | 141:725321fe2949 | 84 | void setDiag(Matrix3f& mat, float val){ |
cocorlow | 141:725321fe2949 | 85 | for (int i = 0; i < mat.cols(); i++){ |
NaotoMorita | 143:53808e4e684c | 86 | for (int j = 0; j < mat.rows(); j++){ |
NaotoMorita | 143:53808e4e684c | 87 | mat(i,j) = 0.0f; |
NaotoMorita | 143:53808e4e684c | 88 | } |
NaotoMorita | 143:53808e4e684c | 89 | } |
NaotoMorita | 143:53808e4e684c | 90 | for (int i = 0; i < mat.cols(); i++){ |
cocorlow | 141:725321fe2949 | 91 | mat(i,i) = val; |
cocorlow | 141:725321fe2949 | 92 | } |
cocorlow | 141:725321fe2949 | 93 | } |
cocorlow | 141:725321fe2949 | 94 | |
cocorlow | 141:725321fe2949 | 95 | void setDiag(MatrixXf& mat, float val){ |
NaotoMorita | 143:53808e4e684c | 96 | for (int i = 0; i < mat.cols(); i++){ |
NaotoMorita | 143:53808e4e684c | 97 | for (int j = 0; j < mat.rows(); j++){ |
NaotoMorita | 143:53808e4e684c | 98 | mat(i,j) = 0.0f; |
NaotoMorita | 143:53808e4e684c | 99 | } |
NaotoMorita | 143:53808e4e684c | 100 | } |
cocorlow | 141:725321fe2949 | 101 | for (int i = 0; i < mat.cols(); i++) |
cocorlow | 141:725321fe2949 | 102 | { |
cocorlow | 141:725321fe2949 | 103 | mat(i, i) = val; |
cocorlow | 141:725321fe2949 | 104 | } |
cocorlow | 141:725321fe2949 | 105 | } |
cocorlow | 141:725321fe2949 | 106 | |
cocorlow | 139:b378528c05f2 | 107 | //void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex){ |
cocorlow | 139:b378528c05f2 | 108 | // for (int i = startIndex; i < endIndex+1; i++){ |
cocorlow | 139:b378528c05f2 | 109 | // mat(i,i) = val; |
cocorlow | 139:b378528c05f2 | 110 | // } |
cocorlow | 139:b378528c05f2 | 111 | //} |