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-18
- Revision:
- 119:a21e283730d1
- Parent:
- 117:f899fd694e2a
- Child:
- 121:2523eef96b36
- Child:
- 122:68efdee114fa
File content as of revision 119:a21e283730d1:
#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}; 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); } }