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@143:53808e4e684c, 22 months ago (annotated)

Committer:
NaotoMorita
Date:
Fri Jun 24 05:44:34 2022 +0000
Revision:
143:53808e4e684c
Parent:
141:725321fe2949 
complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 141:725321fe2949 1 #include "global.hpp"
cocorlow 141:725321fe2949 2 using namespace std;
cocorlow 141:725321fe2949 3
cocorlow 141:725321fe2949 4 void setup()
cocorlow 141:725321fe2949 5 {
cocorlow 141:725321fe2949 6 //sd.baud(115200);
cocorlow 141:725321fe2949 7 //sd.printf("\r\nFlight Start\r\n");
cocorlow 141:725321fe2949 8 //twelite.baud(38400);
cocorlow 141:725321fe2949 9 twelite.serial.printf("\r\nTelemetory Start\r\n");
cocorlow 141:725321fe2949 10 uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G);
cocorlow 141:725321fe2949 11 //printf("%x\n", reg);
cocorlow 141:725321fe2949 12 if (!lps.init()){
cocorlow 141:725321fe2949 13 twelite.serial.printf("Failed to autodetect pressure sensor!\r\n");
cocorlow 141:725321fe2949 14 while (1);
cocorlow 141:725321fe2949 15 }
cocorlow 141:725321fe2949 16 lps.enableDefault();
cocorlow 141:725321fe2949 17 gps.Attach();
cocorlow 141:725321fe2949 18 wait_ms(100);
cocorlow 141:725321fe2949 19
cocorlow 141:725321fe2949 20 SensorAlignmentAG << 1.0f, 0.0f, 0.0f,
cocorlow 141:725321fe2949 21 0.0f, 1.0f, 0.0f,
cocorlow 141:725321fe2949 22 0.0f, 0.0f, -1.0f;
cocorlow 141:725321fe2949 23 SensorAlignmentMAG << -1.0f, 0.0f, 0.0f,
cocorlow 141:725321fe2949 24 0.0f, 1.0f, 0.0f,
cocorlow 141:725321fe2949 25 0.0f, 0.0f, -1.0f;
cocorlow 141:725321fe2949 26 float magMin[3] = {-392.590332, -85.194908, -155.781174};
cocorlow 141:725321fe2949 27 float magMax[3] = {182.602386, 530.811523, 365.834625};
cocorlow 141:725321fe2949 28 magCalibrator.setExtremes(magMin,magMax);
cocorlow 141:725321fe2949 29
cocorlow 141:725321fe2949 30 pitchPID.setSetPoint(0.0f);
cocorlow 141:725321fe2949 31 pitchratePID.setSetPoint(0.0f);
cocorlow 141:725321fe2949 32 rollPID.setSetPoint(0.0f);
cocorlow 141:725321fe2949 33 rollratePID.setSetPoint(0.0f);
cocorlow 141:725321fe2949 34 pitchPID.setBias(0.0f);
cocorlow 141:725321fe2949 35 pitchratePID.setBias(0.0f);
cocorlow 141:725321fe2949 36 rollPID.setBias(0.0f);
cocorlow 141:725321fe2949 37 rollratePID.setBias(0.0f);
cocorlow 141:725321fe2949 38 pitchPID.setOutputLimits(-1.0f,1.0f);
cocorlow 141:725321fe2949 39 pitchratePID.setOutputLimits(-1.0f,1.0f);
cocorlow 141:725321fe2949 40 rollPID.setOutputLimits(-1.0f,1.0f);
cocorlow 141:725321fe2949 41 rollratePID.setOutputLimits(-1.0f,1.0f);
cocorlow 141:725321fe2949 42 pitchPID.setInputLimits(-M_PI_F,M_PI_F);
cocorlow 141:725321fe2949 43 pitchratePID.setInputLimits(-M_PI_F,M_PI_F);
cocorlow 141:725321fe2949 44 rollPID.setInputLimits(-M_PI_F,M_PI_F);
cocorlow 141:725321fe2949 45 rollratePID.setInputLimits(-M_PI_F,M_PI_F);
cocorlow 141:725321fe2949 46
cocorlow 141:725321fe2949 47 servoRight.period_us(15000.0f);
cocorlow 141:725321fe2949 48 servoLeft.period_us(15000.0f);
cocorlow 141:725321fe2949 49 servoThrust.period_us(15000.0f);
cocorlow 141:725321fe2949 50 servoRight.pulsewidth_us(1500.0f);
cocorlow 141:725321fe2949 51 servoLeft.pulsewidth_us(1500.0f);
cocorlow 141:725321fe2949 52 servoThrust.pulsewidth_us(1100.0f);
cocorlow 141:725321fe2949 53
cocorlow 141:725321fe2949 54 autopilot.set_dest(0.0f, 50.0f);
cocorlow 141:725321fe2949 55 autopilot.set_turn(0.0f, 50.0f, 50.0f);
cocorlow 141:725321fe2949 56 autopilot.set_alt(30.0f, 10.0f);
cocorlow 141:725321fe2949 57 autopilot.set_climb(3.0f*M_PI_F/180.0f, 0.0f);
NaotoMorita 143:53808e4e684c 58
cocorlow 141:725321fe2949 59 }
cocorlow 141:725321fe2949 60
cocorlow 141:725321fe2949 61 void calibrate()
cocorlow 141:725321fe2949 62 {
cocorlow 141:725321fe2949 63 while(1)
cocorlow 141:725321fe2949 64 {
cocorlow 141:725321fe2949 65 wait(1000);
cocorlow 141:725321fe2949 66 }
cocorlow 141:725321fe2949 67 }