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
- Committer:
- NaotoMorita
- Date:
- 2021-11-19
- Revision:
- 122:68efdee114fa
- Parent:
- 119:a21e283730d1
File content as of revision 122:68efdee114fa:
#include "global.hpp" using namespace std; void setup() { sd.baud(115200); sd.printf("\r\nFlight Start\r\n"); twelite.baud(38400); twelite.printf("\r\nTelemetory Start\r\n"); uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G); //printf("%x\n", reg); if (!lps.init()){ twelite.printf("Failed to autodetect pressure sensor!\r\n"); while (1); } lps.enableDefault(); gps.Attach(); wait_ms(100); SensorAlignmentAG << 1.0f << 0.0f << 0.0f << 0.0f << 1.0f << 0.0f << 0.0f << 0.0f << -1.0f; SensorAlignmentMAG << -1.0f << 0.0f << 0.0f << 0.0f << 1.0f << 0.0f << 0.0f << 0.0f << -1.0f; float magMin[3] = {-392.590332, -85.194908, -155.781174}; float magMax[3] = {182.602386, 530.811523, 365.834625}; if(hilFlag==true){ for(int i = 0;i<3;i++){ magMin[i] = -1000.0f; magMax[i] = 1000.0f; } } magCalibrator.setExtremes(magMin,magMax); pitchPID.setSetPoint(0.0); pitchratePID.setSetPoint(0.0); rollPID.setSetPoint(0.0); rollratePID.setSetPoint(0.0); pitchPID.setBias(0.0); pitchratePID.setBias(0.0); rollPID.setBias(0.0); rollratePID.setBias(0.0); pitchPID.setOutputLimits(-1.0,1.0); pitchratePID.setOutputLimits(-1.0,1.0); rollPID.setOutputLimits(-1.0,1.0); rollratePID.setOutputLimits(-1.0,1.0); pitchPID.setInputLimits(-M_PI,M_PI); pitchratePID.setInputLimits(-M_PI,M_PI); rollPID.setInputLimits(-M_PI,M_PI); rollratePID.setInputLimits(-M_PI,M_PI); servoRight.period_us(15000.0); servoLeft.period_us(15000.0); servoThrust.period_us(15000.0); servoRight.pulsewidth_us(1500.0); servoLeft.pulsewidth_us(1500.0); servoThrust.pulsewidth_us(1100.0); autopilot.set_dest(destination.x, destination.y); autopilot.set_turn(turn_center.x, turn_center.y, turn_radius); } void calibrate() { while(1) { wait(1000); } }