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
setup.cpp@122:68efdee114fa, 2021-11-19 (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Nov 19 05:44:23 2021 +0000
- Revision:
- 122:68efdee114fa
- Parent:
- 119:a21e283730d1
hatena
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 56:888379912f81 | 1 | #include "global.hpp" |
osaka | 88:be349faa1976 | 2 | using namespace std; |
cocorlow | 56:888379912f81 | 3 | |
cocorlow | 56:888379912f81 | 4 | void setup() |
cocorlow | 56:888379912f81 | 5 | { |
osaka | 88:be349faa1976 | 6 | sd.baud(115200); |
osaka | 88:be349faa1976 | 7 | sd.printf("\r\nFlight Start\r\n"); |
osaka | 88:be349faa1976 | 8 | twelite.baud(38400); |
osaka | 88:be349faa1976 | 9 | twelite.printf("\r\nTelemetory Start\r\n"); |
osaka | 88:be349faa1976 | 10 | |
osaka | 88:be349faa1976 | 11 | uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G); |
osaka | 88:be349faa1976 | 12 | //printf("%x\n", reg); |
osaka | 88:be349faa1976 | 13 | if (!lps.init()){ |
osaka | 88:be349faa1976 | 14 | twelite.printf("Failed to autodetect pressure sensor!\r\n"); |
osaka | 88:be349faa1976 | 15 | while (1); |
osaka | 88:be349faa1976 | 16 | } |
osaka | 88:be349faa1976 | 17 | lps.enableDefault(); |
osaka | 96:3645f8f9bdd3 | 18 | gps.Attach(); |
osaka | 88:be349faa1976 | 19 | wait_ms(100); |
osaka | 88:be349faa1976 | 20 | |
NaotoMorita | 113:3e47d9881529 | 21 | SensorAlignmentAG << 1.0f << 0.0f << 0.0f |
NaotoMorita | 113:3e47d9881529 | 22 | << 0.0f << 1.0f << 0.0f |
NaotoMorita | 113:3e47d9881529 | 23 | << 0.0f << 0.0f << -1.0f; |
NaotoMorita | 119:a21e283730d1 | 24 | SensorAlignmentMAG << -1.0f << 0.0f << 0.0f |
NaotoMorita | 119:a21e283730d1 | 25 | << 0.0f << 1.0f << 0.0f |
NaotoMorita | 113:3e47d9881529 | 26 | << 0.0f << 0.0f << -1.0f; |
NaotoMorita | 122:68efdee114fa | 27 | |
NaotoMorita | 119:a21e283730d1 | 28 | float magMin[3] = {-392.590332, -85.194908, -155.781174}; |
NaotoMorita | 119:a21e283730d1 | 29 | float magMax[3] = {182.602386, 530.811523, 365.834625}; |
NaotoMorita | 122:68efdee114fa | 30 | if(hilFlag==true){ |
NaotoMorita | 122:68efdee114fa | 31 | for(int i = 0;i<3;i++){ |
NaotoMorita | 122:68efdee114fa | 32 | magMin[i] = -1000.0f; |
NaotoMorita | 122:68efdee114fa | 33 | magMax[i] = 1000.0f; |
NaotoMorita | 122:68efdee114fa | 34 | } |
NaotoMorita | 122:68efdee114fa | 35 | } |
NaotoMorita | 119:a21e283730d1 | 36 | magCalibrator.setExtremes(magMin,magMax); |
NaotoMorita | 93:b827f78a717a | 37 | |
cocorlow | 56:888379912f81 | 38 | pitchPID.setSetPoint(0.0); |
cocorlow | 56:888379912f81 | 39 | pitchratePID.setSetPoint(0.0); |
NaotoMorita | 70:99f974d8960e | 40 | rollPID.setSetPoint(0.0); |
NaotoMorita | 70:99f974d8960e | 41 | rollratePID.setSetPoint(0.0); |
cocorlow | 56:888379912f81 | 42 | pitchPID.setBias(0.0); |
NaotoMorita | 70:99f974d8960e | 43 | pitchratePID.setBias(0.0); |
NaotoMorita | 70:99f974d8960e | 44 | rollPID.setBias(0.0); |
NaotoMorita | 70:99f974d8960e | 45 | rollratePID.setBias(0.0); |
cocorlow | 56:888379912f81 | 46 | pitchPID.setOutputLimits(-1.0,1.0); |
cocorlow | 56:888379912f81 | 47 | pitchratePID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 70:99f974d8960e | 48 | rollPID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 70:99f974d8960e | 49 | rollratePID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 70:99f974d8960e | 50 | pitchPID.setInputLimits(-M_PI,M_PI); |
NaotoMorita | 70:99f974d8960e | 51 | pitchratePID.setInputLimits(-M_PI,M_PI); |
NaotoMorita | 70:99f974d8960e | 52 | rollPID.setInputLimits(-M_PI,M_PI); |
NaotoMorita | 70:99f974d8960e | 53 | rollratePID.setInputLimits(-M_PI,M_PI); |
cocorlow | 56:888379912f81 | 54 | |
NaotoMorita | 70:99f974d8960e | 55 | servoRight.period_us(15000.0); |
NaotoMorita | 70:99f974d8960e | 56 | servoLeft.period_us(15000.0); |
NaotoMorita | 70:99f974d8960e | 57 | servoThrust.period_us(15000.0); |
NaotoMorita | 70:99f974d8960e | 58 | servoRight.pulsewidth_us(1500.0); |
NaotoMorita | 70:99f974d8960e | 59 | servoLeft.pulsewidth_us(1500.0); |
NaotoMorita | 70:99f974d8960e | 60 | servoThrust.pulsewidth_us(1100.0); |
osaka | 109:eb255fc4462b | 61 | |
osaka | 109:eb255fc4462b | 62 | autopilot.set_dest(destination.x, destination.y); |
osaka | 109:eb255fc4462b | 63 | autopilot.set_turn(turn_center.x, turn_center.y, turn_radius); |
cocorlow | 56:888379912f81 | 64 | } |
cocorlow | 56:888379912f81 | 65 | |
cocorlow | 56:888379912f81 | 66 | void calibrate() |
cocorlow | 56:888379912f81 | 67 | { |
cocorlow | 56:888379912f81 | 68 | while(1) |
cocorlow | 56:888379912f81 | 69 | { |
cocorlow | 56:888379912f81 | 70 | wait(1000); |
cocorlow | 56:888379912f81 | 71 | } |
cocorlow | 56:888379912f81 | 72 | } |