HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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?

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 {
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 }