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@144:b3a713b4f1c4, 22 months ago (annotated)

Committer:
NaotoMorita
Date:
Wed Jun 29 07:57:10 2022 +0000
Revision:
144:b3a713b4f1c4
Parent:
143:53808e4e684c 
can fly

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
NaotoMorita 144:b3a713b4f1c4 30 pitchPID.setSetPoint(0.0);
NaotoMorita 144:b3a713b4f1c4 31 pitchPID.setBias(0.0);
NaotoMorita 144:b3a713b4f1c4 32 pitchPID.setOutputLimits(-1.0,1.0);
NaotoMorita 144:b3a713b4f1c4 33 pitchPID.setInputLimits(-M_PI,M_PI);
NaotoMorita 144:b3a713b4f1c4 34
NaotoMorita 144:b3a713b4f1c4 35 pitchratePID.setSetPoint(0.0);
NaotoMorita 144:b3a713b4f1c4 36 pitchratePID.setBias(0.0);
NaotoMorita 144:b3a713b4f1c4 37 pitchratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 144:b3a713b4f1c4 38 pitchratePID.setInputLimits(-M_PI,M_PI);
NaotoMorita 144:b3a713b4f1c4 39
NaotoMorita 144:b3a713b4f1c4 40 rollPID.setSetPoint(0.0);
NaotoMorita 144:b3a713b4f1c4 41 rollPID.setBias(0.0);
NaotoMorita 144:b3a713b4f1c4 42 rollPID.setOutputLimits(-1.0,1.0);
NaotoMorita 144:b3a713b4f1c4 43 rollPID.setInputLimits(-M_PI,M_PI);
NaotoMorita 144:b3a713b4f1c4 44
NaotoMorita 144:b3a713b4f1c4 45 rollratePID.setSetPoint(0.0);
NaotoMorita 144:b3a713b4f1c4 46 rollratePID.setBias(0.0);
NaotoMorita 144:b3a713b4f1c4 47 rollratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 144:b3a713b4f1c4 48 rollratePID.setInputLimits(-M_PI,M_PI);
NaotoMorita 144:b3a713b4f1c4 49
NaotoMorita 144:b3a713b4f1c4 50 yawratePID.setSetPoint(0.0);
NaotoMorita 144:b3a713b4f1c4 51 yawratePID.setBias(0.0);
NaotoMorita 144:b3a713b4f1c4 52 yawratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 144:b3a713b4f1c4 53 yawratePID.setInputLimits(-M_PI,M_PI);
cocorlow 141:725321fe2949 54
NaotoMorita 144:b3a713b4f1c4 55 vxPID.setSetPoint(0.0f);
NaotoMorita 144:b3a713b4f1c4 56 vxPID.setBias(0.0f);
NaotoMorita 144:b3a713b4f1c4 57 vxPID.setOutputLimits(-1.0f,1.0);
NaotoMorita 144:b3a713b4f1c4 58 vxPID.setInputLimits(-5.0f,5.0f);
NaotoMorita 144:b3a713b4f1c4 59
NaotoMorita 144:b3a713b4f1c4 60 vyPID.setSetPoint(0.0f);
NaotoMorita 144:b3a713b4f1c4 61 vyPID.setBias(0.0f);
NaotoMorita 144:b3a713b4f1c4 62 vyPID.setOutputLimits(-1.0f,1.0);
NaotoMorita 144:b3a713b4f1c4 63 vyPID.setInputLimits(-5.0f,5.0f);
NaotoMorita 144:b3a713b4f1c4 64
NaotoMorita 144:b3a713b4f1c4 65 vzPID.setSetPoint(0.0f);
NaotoMorita 144:b3a713b4f1c4 66 vzPID.setBias(0.0f);
NaotoMorita 144:b3a713b4f1c4 67 vzPID.setOutputLimits(-1.0f,1.0);
NaotoMorita 144:b3a713b4f1c4 68 vzPID.setInputLimits(-1.0f,1.0f);
cocorlow 141:725321fe2949 69
NaotoMorita 144:b3a713b4f1c4 70
NaotoMorita 144:b3a713b4f1c4 71 motor1.period_us(15000.0);
NaotoMorita 144:b3a713b4f1c4 72 motor2.period_us(15000.0);
NaotoMorita 144:b3a713b4f1c4 73 motor3.period_us(15000.0);
NaotoMorita 144:b3a713b4f1c4 74 motor4.period_us(15000.0);
NaotoMorita 144:b3a713b4f1c4 75 motor5.period_us(15000.0);
NaotoMorita 144:b3a713b4f1c4 76 motor6.period_us(15000.0);
NaotoMorita 144:b3a713b4f1c4 77
NaotoMorita 144:b3a713b4f1c4 78 motor1.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 79 motor2.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 80 motor3.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 81 motor4.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 82 motor5.pulsewidth_us(motorPwmMin);
NaotoMorita 144:b3a713b4f1c4 83 motor6.pulsewidth_us(motorPwmMin);
NaotoMorita 143:53808e4e684c 84
cocorlow 141:725321fe2949 85 }
cocorlow 141:725321fe2949 86
cocorlow 141:725321fe2949 87 void calibrate()
cocorlow 141:725321fe2949 88 {
cocorlow 141:725321fe2949 89 while(1)
cocorlow 141:725321fe2949 90 {
cocorlow 141:725321fe2949 91 wait(1000);
cocorlow 141:725321fe2949 92 }
cocorlow 141:725321fe2949 93 }