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
- Committer:
- osaka
- Date:
- 2021-11-12
- Revision:
- 107:46e039e12182
- Parent:
- 96:3645f8f9bdd3
File content as of revision 107:46e039e12182:
#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); SensorAlignment << 1.0f << 0.0f << 0.0f << 0.0f << 1.0f << 0.0f << 0.0f << 0.0f << -1.0f; 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); autopilot.set_dest(destination.x, destination.y); autopilot.set_turn(turn_center.x, turn_center.y, turn_radius); 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); } void calibrate() { while(1) { wait(1000); } }