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
global.cpp@112:6a33ea9f6fed, 2021-11-16 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Nov 16 01:21:01 2021 +0000
- Revision:
- 112:6a33ea9f6fed
- Parent:
- 111:0fae4fbe2a80
- Child:
- 113:3e47d9881529
magmod
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 | 76:7fd3ac1afe3e | 8 | Serial sd(PG_14,PG_9); |
osaka | 88:be349faa1976 | 9 | Serial twelite(PD_1,PD_0); |
cocorlow | 56:888379912f81 | 10 | |
cocorlow | 56:888379912f81 | 11 | // io |
cocorlow | 56:888379912f81 | 12 | DigitalIn userButton(USER_BUTTON); |
NaotoMorita | 70:99f974d8960e | 13 | SBUS sbus(PD_5, PD_6); |
osaka | 88:be349faa1976 | 14 | I2C i2c(PB_9, PB_8); |
osaka | 88:be349faa1976 | 15 | LSM9DS1 lsm(i2c); |
osaka | 88:be349faa1976 | 16 | LPS lps(i2c); |
NaotoMorita | 99:98b892ca70ec | 17 | GPSUBX_UART gps(PF_7, PE_7); |
NaotoMorita | 104:20b8caa29185 | 18 | MagSphereCalibration magCalibrator; |
NaotoMorita | 102:1c77ff6e2a85 | 19 | float magres = 0.0f; |
cocorlow | 56:888379912f81 | 20 | // control |
NaotoMorita | 94:579e875a4244 | 21 | Timer _t; |
NaotoMorita | 70:99f974d8960e | 22 | FastPWM servoRight(PE_9); |
NaotoMorita | 70:99f974d8960e | 23 | FastPWM servoLeft(PE_11); |
NaotoMorita | 70:99f974d8960e | 24 | FastPWM servoThrust(PE_13); |
NaotoMorita | 74:f67062e7813e | 25 | PID pitchPID(10.0f,0.0f,0.0f,PID_dt); //rad |
NaotoMorita | 74:f67062e7813e | 26 | PID pitchratePID(1.0f, 0.0f, 0.0f, PID_dt);//rad/s |
NaotoMorita | 74:f67062e7813e | 27 | PID rollPID(5.0f,0.0f,0.0f,PID_dt); |
NaotoMorita | 94:579e875a4244 | 28 | PID rollratePID(0.05f, 0.0, 0.0, PID_dt);//rad/s |
NaotoMorita | 90:96c2b0ed4b96 | 29 | solaESKF eskf; // ESKF class |
NaotoMorita | 76:7fd3ac1afe3e | 30 | int obsCount = 0; |
osaka | 109:eb255fc4462b | 31 | Autopilot autopilot; |
osaka | 109:eb255fc4462b | 32 | float roll_obj; |
osaka | 109:eb255fc4462b | 33 | float pitch_obj; |
osaka | 109:eb255fc4462b | 34 | float alt_obj; |
osaka | 109:eb255fc4462b | 35 | Vector3 destination(100.0f, 100.0f, 0.0f); |
osaka | 109:eb255fc4462b | 36 | Vector3 turn_center(100.0f, 100.0f, 0.0f); |
osaka | 109:eb255fc4462b | 37 | float turn_radius = 50.0f; |
cocorlow | 56:888379912f81 | 38 | |
NaotoMorita | 70:99f974d8960e | 39 | float rc[16]; |
cocorlow | 56:888379912f81 | 40 | int loop_count = 0; |
cocorlow | 56:888379912f81 | 41 | float att_dt = 0.01f; |
NaotoMorita | 112:6a33ea9f6fed | 42 | bool accmagSwitch = true; |
cocorlow | 56:888379912f81 | 43 | // position |
NaotoMorita | 93:b827f78a717a | 44 | Matrix SensorAlignment(3,3); |
cocorlow | 56:888379912f81 | 45 | Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll y:pitch z:yaw |
cocorlow | 56:888379912f81 | 46 | Vector3 acc; |
NaotoMorita | 102:1c77ff6e2a85 | 47 | Vector3 accref(0.0f, 0.0f, 9.8f); |
cocorlow | 56:888379912f81 | 48 | Vector3 mag; |
NaotoMorita | 102:1c77ff6e2a85 | 49 | Vector3 magref(0.0f, 0.0f, 0.0f); |
cocorlow | 56:888379912f81 | 50 | Vector3 gyro; |
NaotoMorita | 82:c183c29d2427 | 51 | Vector3 vi; |
NaotoMorita | 92:00460f6df439 | 52 | Vector3 pi; |
NaotoMorita | 92:00460f6df439 | 53 | float palt; |
NaotoMorita | 98:bdaa6bbfb1d9 | 54 | float palt0; |
NaotoMorita | 106:2d854e92cebb | 55 | int itow_status = 0; |
NaotoMorita | 100:7589b663d462 | 56 | bool gpsUpdateFlag = false; |
NaotoMorita | 106:2d854e92cebb | 57 | bool gpsLlh0Fixed = false; |
NaotoMorita | 77:2bf856e3eca4 | 58 | |
NaotoMorita | 73:84ffa0166e6c | 59 | float de = 0.0f; |
NaotoMorita | 73:84ffa0166e6c | 60 | float da = 0.0f; |
NaotoMorita | 73:84ffa0166e6c | 61 | float dT = 0.0f; |
NaotoMorita | 70:99f974d8960e | 62 | MedianFilter accMedian(3); |
NaotoMorita | 70:99f974d8960e | 63 | MedianFilter gyroMedian(3); |
NaotoMorita | 70:99f974d8960e | 64 | MedianFilter magMedian(3); |
NaotoMorita | 72:267e2cfddb0b | 65 | |
NaotoMorita | 70:99f974d8960e | 66 | float scaledServoOut[2]= {0.0f,0.0f}; |
NaotoMorita | 70:99f974d8960e | 67 | float scaledMotorOut[1]= {-1.0f}; |
NaotoMorita | 63:9c4973a98600 | 68 | float servoOut[2] = {1500.0f,1500.0f}; |
NaotoMorita | 70:99f974d8960e | 69 | float motorOut[1] = {1100.0f}; |
cocorlow | 56:888379912f81 | 70 | |
cocorlow | 56:888379912f81 | 71 | int calibrationFlag = 0; |
NaotoMorita | 93:b827f78a717a | 72 | float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; |
NaotoMorita | 76:7fd3ac1afe3e | 73 | |
NaotoMorita | 76:7fd3ac1afe3e | 74 | float magbias[4] = {-134.817047, 127.833481, -152.631836, 44.508556}; |
NaotoMorita | 75:a505b9896da1 | 75 | float magbiasMin[3] = {-174.831711, 93.465691, -188.617172}; |
NaotoMorita | 75:a505b9896da1 | 76 | float magbiasMax[3] = {-110.413963, 162.321548, -122.566071}; |
NaotoMorita | 75:a505b9896da1 | 77 | float accMin[3] = {-0.963692, -0.974141, -1.012899}; |
NaotoMorita | 75:a505b9896da1 | 78 | float accMax[3] = { 1.025954, 0.974748, 0.987155}; |
cocorlow | 56:888379912f81 | 79 | |
NaotoMorita | 94:579e875a4244 | 80 | Vector3 rpy_align( 0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f); |
cocorlow | 56:888379912f81 | 81 | |
cocorlow | 56:888379912f81 | 82 | |
cocorlow | 56:888379912f81 | 83 | // UsaPack |
NaotoMorita | 73:84ffa0166e6c | 84 | valuePack vp; |
NaotoMorita | 83:e69ab831031c | 85 | sendPack sp; |
NaotoMorita | 73:84ffa0166e6c | 86 | |
NaotoMorita | 73:84ffa0166e6c | 87 | // HIL |
NaotoMorita | 111:0fae4fbe2a80 | 88 | bool hilFlag = false; |
NaotoMorita | 76:7fd3ac1afe3e | 89 | bool serialControlSource = false; |
NaotoMorita | 76:7fd3ac1afe3e | 90 | bool serialParamSource = false; |
NaotoMorita | 76:7fd3ac1afe3e | 91 | int checkParamSerial[5] = {0,0,0,0,0}; |
cocorlow | 56:888379912f81 | 92 | |
cocorlow | 56:888379912f81 | 93 | float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) |
cocorlow | 56:888379912f81 | 94 | { |
cocorlow | 56:888379912f81 | 95 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
NaotoMorita | 67:41fcdfb7cc5a | 96 | } |
NaotoMorita | 93:b827f78a717a | 97 | |
NaotoMorita | 93:b827f78a717a | 98 | void setDiag(Matrix& mat, float val){ |
NaotoMorita | 93:b827f78a717a | 99 | for (int i = 1; i < mat.getCols()+1; i++){ |
NaotoMorita | 93:b827f78a717a | 100 | mat(i,i) = val; |
NaotoMorita | 93:b827f78a717a | 101 | } |
NaotoMorita | 93:b827f78a717a | 102 | } |
NaotoMorita | 93:b827f78a717a | 103 | |
NaotoMorita | 93:b827f78a717a | 104 | void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){ |
NaotoMorita | 93:b827f78a717a | 105 | for (int i = startIndex; i < endIndex+1; i++){ |
NaotoMorita | 93:b827f78a717a | 106 | mat(i,i) = val; |
NaotoMorita | 93:b827f78a717a | 107 | } |
NaotoMorita | 93:b827f78a717a | 108 | } |