HAPSRG / Mbed 2 deprecated hexaTest_Eigen

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@137:240588882ae2, 2021-12-01 (annotated)

Committer:
osaka
Date:
Wed Dec 01 11:51:49 2021 +0000
Revision:
137:240588882ae2
Parent:
134:d57c6b2a706b
Child:
139:b378528c05f2
climb

Who changed what in which revision?

UserRevisionLine numberNew 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 {
NaotoMorita 132:896ad37b534b 6 //sd.baud(115200);
NaotoMorita 132:896ad37b534b 7 //sd.printf("\r\nFlight Start\r\n");
NaotoMorita 133:346ce20b3950 8 //twelite.baud(38400);
NaotoMorita 134:d57c6b2a706b 9 twelite.serial.printf("\r\nTelemetory Start\r\n");
osaka 88:be349faa1976 10 uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G);
osaka 88:be349faa1976 11 //printf("%x\n", reg);
osaka 88:be349faa1976 12 if (!lps.init()){
NaotoMorita 134:d57c6b2a706b 13 twelite.serial.printf("Failed to autodetect pressure sensor!\r\n");
osaka 88:be349faa1976 14 while (1);
osaka 88:be349faa1976 15 }
osaka 88:be349faa1976 16 lps.enableDefault();
osaka 96:3645f8f9bdd3 17 gps.Attach();
osaka 88:be349faa1976 18 wait_ms(100);
osaka 88:be349faa1976 19
NaotoMorita 113:3e47d9881529 20 SensorAlignmentAG << 1.0f << 0.0f << 0.0f
NaotoMorita 113:3e47d9881529 21 << 0.0f << 1.0f << 0.0f
NaotoMorita 113:3e47d9881529 22 << 0.0f << 0.0f << -1.0f;
NaotoMorita 119:a21e283730d1 23 SensorAlignmentMAG << -1.0f << 0.0f << 0.0f
NaotoMorita 119:a21e283730d1 24 << 0.0f << 1.0f << 0.0f
NaotoMorita 113:3e47d9881529 25 << 0.0f << 0.0f << -1.0f;
NaotoMorita 119:a21e283730d1 26 float magMin[3] = {-392.590332, -85.194908, -155.781174};
NaotoMorita 119:a21e283730d1 27 float magMax[3] = {182.602386, 530.811523, 365.834625};
NaotoMorita 119:a21e283730d1 28 magCalibrator.setExtremes(magMin,magMax);
NaotoMorita 93:b827f78a717a 29
cocorlow 56:888379912f81 30 pitchPID.setSetPoint(0.0);
cocorlow 56:888379912f81 31 pitchratePID.setSetPoint(0.0);
NaotoMorita 70:99f974d8960e 32 rollPID.setSetPoint(0.0);
NaotoMorita 70:99f974d8960e 33 rollratePID.setSetPoint(0.0);
cocorlow 56:888379912f81 34 pitchPID.setBias(0.0);
NaotoMorita 70:99f974d8960e 35 pitchratePID.setBias(0.0);
NaotoMorita 70:99f974d8960e 36 rollPID.setBias(0.0);
NaotoMorita 70:99f974d8960e 37 rollratePID.setBias(0.0);
cocorlow 56:888379912f81 38 pitchPID.setOutputLimits(-1.0,1.0);
cocorlow 56:888379912f81 39 pitchratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 70:99f974d8960e 40 rollPID.setOutputLimits(-1.0,1.0);
NaotoMorita 70:99f974d8960e 41 rollratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 70:99f974d8960e 42 pitchPID.setInputLimits(-M_PI,M_PI);
NaotoMorita 70:99f974d8960e 43 pitchratePID.setInputLimits(-M_PI,M_PI);
NaotoMorita 70:99f974d8960e 44 rollPID.setInputLimits(-M_PI,M_PI);
NaotoMorita 70:99f974d8960e 45 rollratePID.setInputLimits(-M_PI,M_PI);
cocorlow 56:888379912f81 46
NaotoMorita 70:99f974d8960e 47 servoRight.period_us(15000.0);
NaotoMorita 70:99f974d8960e 48 servoLeft.period_us(15000.0);
NaotoMorita 70:99f974d8960e 49 servoThrust.period_us(15000.0);
NaotoMorita 70:99f974d8960e 50 servoRight.pulsewidth_us(1500.0);
NaotoMorita 70:99f974d8960e 51 servoLeft.pulsewidth_us(1500.0);
NaotoMorita 70:99f974d8960e 52 servoThrust.pulsewidth_us(1100.0);
osaka 109:eb255fc4462b 53
NaotoMorita 129:a76be8380bb2 54 autopilot.set_dest(0.0f, 50.0f);
NaotoMorita 129:a76be8380bb2 55 autopilot.set_turn(0.0f, 50.0f, 50.0f);
osaka 121:2523eef96b36 56 autopilot.set_alt(30.0f, 10.0f);
osaka 137:240588882ae2 57 autopilot.set_climb(3.0f*M_PI/180.0f, 0);
cocorlow 56:888379912f81 58 }
cocorlow 56:888379912f81 59
cocorlow 56:888379912f81 60 void calibrate()
cocorlow 56:888379912f81 61 {
cocorlow 56:888379912f81 62 while(1)
cocorlow 56:888379912f81 63 {
cocorlow 56:888379912f81 64 wait(1000);
cocorlow 56:888379912f81 65 }
cocorlow 56:888379912f81 66 }