solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
setup.cpp@139:b378528c05f2, 2021-12-06 (annotated)
- Committer:
- cocorlow
- Date:
- Mon Dec 06 08:26:16 2021 +0000
- Revision:
- 139:b378528c05f2
- Parent:
- 137:240588882ae2
- Child:
- 141:725321fe2949
Eigen
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 139:b378528c05f2 | 1 | //#include "global.hpp" |
cocorlow | 139:b378528c05f2 | 2 | //using namespace std; |
cocorlow | 139:b378528c05f2 | 3 | // |
cocorlow | 139:b378528c05f2 | 4 | //void setup() |
cocorlow | 139:b378528c05f2 | 5 | //{ |
cocorlow | 139:b378528c05f2 | 6 | // //sd.baud(115200); |
cocorlow | 139:b378528c05f2 | 7 | // //sd.printf("\r\nFlight Start\r\n"); |
cocorlow | 139:b378528c05f2 | 8 | // //twelite.baud(38400); |
cocorlow | 139:b378528c05f2 | 9 | // twelite.serial.printf("\r\nTelemetory Start\r\n"); |
cocorlow | 139:b378528c05f2 | 10 | // uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G); |
cocorlow | 139:b378528c05f2 | 11 | // //printf("%x\n", reg); |
cocorlow | 139:b378528c05f2 | 12 | // if (!lps.init()){ |
cocorlow | 139:b378528c05f2 | 13 | // twelite.serial.printf("Failed to autodetect pressure sensor!\r\n"); |
cocorlow | 139:b378528c05f2 | 14 | // while (1); |
cocorlow | 139:b378528c05f2 | 15 | // } |
cocorlow | 139:b378528c05f2 | 16 | // lps.enableDefault(); |
cocorlow | 139:b378528c05f2 | 17 | // gps.Attach(); |
cocorlow | 139:b378528c05f2 | 18 | // wait_ms(100); |
cocorlow | 139:b378528c05f2 | 19 | // |
cocorlow | 139:b378528c05f2 | 20 | // SensorAlignmentAG << 1.0f << 0.0f << 0.0f |
cocorlow | 139:b378528c05f2 | 21 | // << 0.0f << 1.0f << 0.0f |
cocorlow | 139:b378528c05f2 | 22 | // << 0.0f << 0.0f << -1.0f; |
cocorlow | 139:b378528c05f2 | 23 | // SensorAlignmentMAG << -1.0f << 0.0f << 0.0f |
cocorlow | 139:b378528c05f2 | 24 | // << 0.0f << 1.0f << 0.0f |
cocorlow | 139:b378528c05f2 | 25 | // << 0.0f << 0.0f << -1.0f; |
cocorlow | 139:b378528c05f2 | 26 | // float magMin[3] = {-392.590332, -85.194908, -155.781174}; |
cocorlow | 139:b378528c05f2 | 27 | // float magMax[3] = {182.602386, 530.811523, 365.834625}; |
cocorlow | 139:b378528c05f2 | 28 | // magCalibrator.setExtremes(magMin,magMax); |
cocorlow | 139:b378528c05f2 | 29 | // |
cocorlow | 139:b378528c05f2 | 30 | // pitchPID.setSetPoint(0.0); |
cocorlow | 139:b378528c05f2 | 31 | // pitchratePID.setSetPoint(0.0); |
cocorlow | 139:b378528c05f2 | 32 | // rollPID.setSetPoint(0.0); |
cocorlow | 139:b378528c05f2 | 33 | // rollratePID.setSetPoint(0.0); |
cocorlow | 139:b378528c05f2 | 34 | // pitchPID.setBias(0.0); |
cocorlow | 139:b378528c05f2 | 35 | // pitchratePID.setBias(0.0); |
cocorlow | 139:b378528c05f2 | 36 | // rollPID.setBias(0.0); |
cocorlow | 139:b378528c05f2 | 37 | // rollratePID.setBias(0.0); |
cocorlow | 139:b378528c05f2 | 38 | // pitchPID.setOutputLimits(-1.0,1.0); |
cocorlow | 139:b378528c05f2 | 39 | // pitchratePID.setOutputLimits(-1.0,1.0); |
cocorlow | 139:b378528c05f2 | 40 | // rollPID.setOutputLimits(-1.0,1.0); |
cocorlow | 139:b378528c05f2 | 41 | // rollratePID.setOutputLimits(-1.0,1.0); |
cocorlow | 139:b378528c05f2 | 42 | // pitchPID.setInputLimits(-M_PI,M_PI); |
cocorlow | 139:b378528c05f2 | 43 | // pitchratePID.setInputLimits(-M_PI,M_PI); |
cocorlow | 139:b378528c05f2 | 44 | // rollPID.setInputLimits(-M_PI,M_PI); |
cocorlow | 139:b378528c05f2 | 45 | // rollratePID.setInputLimits(-M_PI,M_PI); |
cocorlow | 139:b378528c05f2 | 46 | // |
cocorlow | 139:b378528c05f2 | 47 | // servoRight.period_us(15000.0); |
cocorlow | 139:b378528c05f2 | 48 | // servoLeft.period_us(15000.0); |
cocorlow | 139:b378528c05f2 | 49 | // servoThrust.period_us(15000.0); |
cocorlow | 139:b378528c05f2 | 50 | // servoRight.pulsewidth_us(1500.0); |
cocorlow | 139:b378528c05f2 | 51 | // servoLeft.pulsewidth_us(1500.0); |
cocorlow | 139:b378528c05f2 | 52 | // servoThrust.pulsewidth_us(1100.0); |
cocorlow | 139:b378528c05f2 | 53 | // |
cocorlow | 139:b378528c05f2 | 54 | // autopilot.set_dest(0.0f, 50.0f); |
cocorlow | 139:b378528c05f2 | 55 | // autopilot.set_turn(0.0f, 50.0f, 50.0f); |
cocorlow | 139:b378528c05f2 | 56 | // autopilot.set_alt(30.0f, 10.0f); |
cocorlow | 139:b378528c05f2 | 57 | // autopilot.set_climb(3.0f*M_PI/180.0f, 0); |
cocorlow | 139:b378528c05f2 | 58 | //} |
cocorlow | 139:b378528c05f2 | 59 | // |
cocorlow | 139:b378528c05f2 | 60 | //void calibrate() |
cocorlow | 139:b378528c05f2 | 61 | //{ |
cocorlow | 139:b378528c05f2 | 62 | // while(1) |
cocorlow | 139:b378528c05f2 | 63 | // { |
cocorlow | 139:b378528c05f2 | 64 | // wait(1000); |
cocorlow | 139:b378528c05f2 | 65 | // } |
cocorlow | 139:b378528c05f2 | 66 | //} |