solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: setup.cpp
- Revision:
- 144:b3a713b4f1c4
- Parent:
- 143:53808e4e684c
--- a/setup.cpp Fri Jun 24 05:44:34 2022 +0000 +++ b/setup.cpp Wed Jun 29 07:57:10 2022 +0000 @@ -27,34 +27,60 @@ 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); + pitchPID.setSetPoint(0.0); + pitchPID.setBias(0.0); + pitchPID.setOutputLimits(-1.0,1.0); + pitchPID.setInputLimits(-M_PI,M_PI); + + pitchratePID.setSetPoint(0.0); + pitchratePID.setBias(0.0); + pitchratePID.setOutputLimits(-1.0,1.0); + pitchratePID.setInputLimits(-M_PI,M_PI); + + rollPID.setSetPoint(0.0); + rollPID.setBias(0.0); + rollPID.setOutputLimits(-1.0,1.0); + rollPID.setInputLimits(-M_PI,M_PI); + + rollratePID.setSetPoint(0.0); + rollratePID.setBias(0.0); + rollratePID.setOutputLimits(-1.0,1.0); + rollratePID.setInputLimits(-M_PI,M_PI); + + yawratePID.setSetPoint(0.0); + yawratePID.setBias(0.0); + yawratePID.setOutputLimits(-1.0,1.0); + yawratePID.setInputLimits(-M_PI,M_PI); - 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); + vxPID.setSetPoint(0.0f); + vxPID.setBias(0.0f); + vxPID.setOutputLimits(-1.0f,1.0); + vxPID.setInputLimits(-5.0f,5.0f); + + vyPID.setSetPoint(0.0f); + vyPID.setBias(0.0f); + vyPID.setOutputLimits(-1.0f,1.0); + vyPID.setInputLimits(-5.0f,5.0f); + + vzPID.setSetPoint(0.0f); + vzPID.setBias(0.0f); + vzPID.setOutputLimits(-1.0f,1.0); + vzPID.setInputLimits(-1.0f,1.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); + + motor1.period_us(15000.0); + motor2.period_us(15000.0); + motor3.period_us(15000.0); + motor4.period_us(15000.0); + motor5.period_us(15000.0); + motor6.period_us(15000.0); + + motor1.pulsewidth_us(motorPwmMin); + motor2.pulsewidth_us(motorPwmMin); + motor3.pulsewidth_us(motorPwmMin); + motor4.pulsewidth_us(motorPwmMin); + motor5.pulsewidth_us(motorPwmMin); + motor6.pulsewidth_us(motorPwmMin); }