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
Diff: global.cpp
- Revision:
- 70:99f974d8960e
- Parent:
- 68:b9f6938fab9d
- Child:
- 72:267e2cfddb0b
diff -r 401c5d4454fc -r 99f974d8960e global.cpp --- a/global.cpp Mon Jun 28 01:45:12 2021 +0000 +++ b/global.cpp Tue Jun 29 08:07:55 2021 +0000 @@ -14,17 +14,21 @@ // io DigitalIn userButton(USER_BUTTON); -CalibrateMagneto joyCalibrator; +SBUS sbus(PD_5, PD_6); + // control -FastPWM elevServo(PE_11); -FastPWM rudServo(PE_13); -PID pitchPID(5.0, 0,0, PID_dt); // rad -PID pitchratePID(0.5, 0.0, 0.0, PID_dt); // rad/s +FastPWM servoRight(PE_9); +FastPWM servoLeft(PE_11); +FastPWM servoThrust(PE_13); +PID pitchPID(5.0, 0,0,PID_dt); //rad +PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s +PID rollPID(5.0,0.0,0.0,PID_dt); +PID rollratePID(5, 0.0, 0.0, PID_dt);//rad/s errStateEKF ekf; // EKF class +float rc[16]; int loop_count = 0; float att_dt = 0.01f; - int16_t ax, ay, az; int16_t gx, gy, gz; MotionSensorDataUnits mdata; @@ -38,16 +42,23 @@ Vector3 mag; Vector3 magref(0.65f, 0.0f, 0.75f); Vector3 gyro; - -float scaledServoOut[2] = {0.0f,0.0f}; +MedianFilter accMedian(3); +MedianFilter gyroMedian(3); +MedianFilter magMedian(3); +float scaledServoOut[2]= {0.0f,0.0f}; +float scaledMotorOut[1]= {-1.0f}; float servoOut[2] = {1500.0f,1500.0f}; +float motorOut[1] = {1100.0f}; int calibrationFlag = 0; -int agoffset[6] = {0, 0, 0, -121, -12, 24}; -float magbiasMin[3] = {-45.639572, -0.059049, -208.120300}; -float magbiasMax[3] = {28.209938, 65.421822, -131.388748}; +int agoffset[6] = {0, 0, 0, -585, -83, 38 }; +float magbiasMin[3] = {-177.574768, 90.129456, -199.611603}; +float magbiasMax[3] = {-108.064606, 160.956711, -125.601234}; +float accMin[3] = {-0.958801, -1.016878, -1.054199}; +float accMax[3] = { 1.050249, 0.965697, 0.946194}; -Vector3 rpy_align(-1.192014f*M_PI/180.0f, -2.639991f*M_PI/180.0f, 0.0f*M_PI/180.0f); +Vector3 rpy_align(8.798061f*M_PI/180.0f, 0.953862f*M_PI/180.0f, 0.0f*M_PI/180.0f +); // UsaPack