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:
- cocorlow
- Date:
- 2021-12-10
- Revision:
- 141:725321fe2949
- Parent:
- 139:b378528c05f2
- Child:
- 143:53808e4e684c
File content as of revision 141:725321fe2949:
#include "global.hpp" using namespace std; void setup() { //sd.baud(115200); //sd.printf("\r\nFlight Start\r\n"); //twelite.baud(38400); twelite.serial.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.serial.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.0f); pitchratePID.setSetPoint(0.0f); rollPID.setSetPoint(0.0f); rollratePID.setSetPoint(0.0f); pitchPID.setBias(0.0f); pitchratePID.setBias(0.0f); rollPID.setBias(0.0f); rollratePID.setBias(0.0f); pitchPID.setOutputLimits(-1.0f,1.0f); pitchratePID.setOutputLimits(-1.0f,1.0f); rollPID.setOutputLimits(-1.0f,1.0f); rollratePID.setOutputLimits(-1.0f,1.0f); pitchPID.setInputLimits(-M_PI_F,M_PI_F); pitchratePID.setInputLimits(-M_PI_F,M_PI_F); rollPID.setInputLimits(-M_PI_F,M_PI_F); rollratePID.setInputLimits(-M_PI_F,M_PI_F); servoRight.period_us(15000.0f); servoLeft.period_us(15000.0f); servoThrust.period_us(15000.0f); servoRight.pulsewidth_us(1500.0f); servoLeft.pulsewidth_us(1500.0f); servoThrust.pulsewidth_us(1100.0f); autopilot.set_dest(0.0f, 50.0f); autopilot.set_turn(0.0f, 50.0f, 50.0f); autopilot.set_alt(30.0f, 10.0f); autopilot.set_climb(3.0f*M_PI_F/180.0f, 0.0f); } void calibrate() { while(1) { wait(1000); } }