solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
global.cpp@73:84ffa0166e6c, 2021-07-15 (annotated)
- Committer:
- NaotoMorita
- Date:
- Thu Jul 15 05:26:15 2021 +0000
- Revision:
- 73:84ffa0166e6c
- Parent:
- 72:267e2cfddb0b
- Child:
- 74:f67062e7813e
9state
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 56:888379912f81 | 1 | #include "global.hpp" |
cocorlow | 56:888379912f81 | 2 | |
NaotoMorita | 67:41fcdfb7cc5a | 3 | |
cocorlow | 56:888379912f81 | 4 | // var |
cocorlow | 56:888379912f81 | 5 | |
cocorlow | 56:888379912f81 | 6 | // communication |
cocorlow | 56:888379912f81 | 7 | I2C i2c(PB_9,PB_8); // sda, scl |
NaotoMorita | 67:41fcdfb7cc5a | 8 | UsaPack pc(USBTX, USBRX, 57600); // log - tail |
cocorlow | 56:888379912f81 | 9 | |
cocorlow | 56:888379912f81 | 10 | // sensor |
cocorlow | 56:888379912f81 | 11 | MPU6050 accelgyro; |
cocorlow | 56:888379912f81 | 12 | MAG3110 mag_sensor(PB_9,PB_8); |
NaotoMorita | 61:c05353850017 | 13 | CalibrateMagneto magCalibrator; |
cocorlow | 56:888379912f81 | 14 | |
cocorlow | 56:888379912f81 | 15 | // io |
cocorlow | 56:888379912f81 | 16 | DigitalIn userButton(USER_BUTTON); |
NaotoMorita | 70:99f974d8960e | 17 | SBUS sbus(PD_5, PD_6); |
NaotoMorita | 73:84ffa0166e6c | 18 | Joystick joystick(PC_3,PF_3,PF_5); |
NaotoMorita | 70:99f974d8960e | 19 | |
cocorlow | 56:888379912f81 | 20 | // control |
NaotoMorita | 70:99f974d8960e | 21 | FastPWM servoRight(PE_9); |
NaotoMorita | 70:99f974d8960e | 22 | FastPWM servoLeft(PE_11); |
NaotoMorita | 70:99f974d8960e | 23 | FastPWM servoThrust(PE_13); |
NaotoMorita | 73:84ffa0166e6c | 24 | PID pitchPID(20.0f*0.6f,20.0f*1.2f/2.0f,0.0,PID_dt); //rad |
NaotoMorita | 73:84ffa0166e6c | 25 | PID pitchratePID(0.075f*20.0f*2.0f, 0.0, 0.0, PID_dt);//rad/s |
NaotoMorita | 73:84ffa0166e6c | 26 | PID rollPID(6.0f*0.6f,6.0f*1.2f/1.0f,0.0,PID_dt); |
NaotoMorita | 73:84ffa0166e6c | 27 | PID rollratePID(0.075f*6.0f*1.0f, 0.0, 0.0, PID_dt);//rad/s |
NaotoMorita | 67:41fcdfb7cc5a | 28 | errStateEKF ekf; // EKF class |
cocorlow | 56:888379912f81 | 29 | |
NaotoMorita | 70:99f974d8960e | 30 | float rc[16]; |
cocorlow | 56:888379912f81 | 31 | int loop_count = 0; |
cocorlow | 56:888379912f81 | 32 | float att_dt = 0.01f; |
cocorlow | 56:888379912f81 | 33 | int16_t ax, ay, az; |
cocorlow | 56:888379912f81 | 34 | int16_t gx, gy, gz; |
cocorlow | 56:888379912f81 | 35 | MotionSensorDataUnits mdata; |
cocorlow | 56:888379912f81 | 36 | float magval[3] = {1.0f, 0.0f, 0.0f}; |
cocorlow | 56:888379912f81 | 37 | |
cocorlow | 56:888379912f81 | 38 | // position |
cocorlow | 56:888379912f81 | 39 | Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll y:pitch z:yaw |
cocorlow | 56:888379912f81 | 40 | Vector3 acc; |
NaotoMorita | 68:b9f6938fab9d | 41 | Vector3 accref(0.0f, 0.0f, 0.98f); |
NaotoMorita | 68:b9f6938fab9d | 42 | Vector3 dynacc; |
cocorlow | 56:888379912f81 | 43 | Vector3 mag; |
cocorlow | 56:888379912f81 | 44 | Vector3 magref(0.65f, 0.0f, 0.75f); |
cocorlow | 56:888379912f81 | 45 | Vector3 gyro; |
NaotoMorita | 73:84ffa0166e6c | 46 | float de = 0.0f; |
NaotoMorita | 73:84ffa0166e6c | 47 | float da = 0.0f; |
NaotoMorita | 73:84ffa0166e6c | 48 | float dT = 0.0f; |
NaotoMorita | 70:99f974d8960e | 49 | MedianFilter accMedian(3); |
NaotoMorita | 70:99f974d8960e | 50 | MedianFilter gyroMedian(3); |
NaotoMorita | 70:99f974d8960e | 51 | MedianFilter magMedian(3); |
NaotoMorita | 72:267e2cfddb0b | 52 | |
NaotoMorita | 70:99f974d8960e | 53 | float scaledServoOut[2]= {0.0f,0.0f}; |
NaotoMorita | 70:99f974d8960e | 54 | float scaledMotorOut[1]= {-1.0f}; |
NaotoMorita | 63:9c4973a98600 | 55 | float servoOut[2] = {1500.0f,1500.0f}; |
NaotoMorita | 70:99f974d8960e | 56 | float motorOut[1] = {1100.0f}; |
cocorlow | 56:888379912f81 | 57 | |
cocorlow | 56:888379912f81 | 58 | int calibrationFlag = 0; |
NaotoMorita | 73:84ffa0166e6c | 59 | int agoffset[6] = {0, 0, 0, -616, -108, 60}; |
NaotoMorita | 70:99f974d8960e | 60 | float magbiasMin[3] = {-177.574768, 90.129456, -199.611603}; |
NaotoMorita | 70:99f974d8960e | 61 | float magbiasMax[3] = {-108.064606, 160.956711, -125.601234}; |
NaotoMorita | 70:99f974d8960e | 62 | float accMin[3] = {-0.958801, -1.016878, -1.054199}; |
NaotoMorita | 70:99f974d8960e | 63 | float accMax[3] = { 1.050249, 0.965697, 0.946194}; |
cocorlow | 56:888379912f81 | 64 | |
NaotoMorita | 73:84ffa0166e6c | 65 | Vector3 rpy_align(8.798061f*M_PI/180.0f, 0.953862f*M_PI/180.0f, 0.0f*M_PI/180.0f); |
cocorlow | 56:888379912f81 | 66 | |
cocorlow | 56:888379912f81 | 67 | |
cocorlow | 56:888379912f81 | 68 | // UsaPack |
NaotoMorita | 73:84ffa0166e6c | 69 | valuePack vp; |
NaotoMorita | 73:84ffa0166e6c | 70 | |
NaotoMorita | 73:84ffa0166e6c | 71 | // HIL |
NaotoMorita | 73:84ffa0166e6c | 72 | bool hilFlag = true; |
cocorlow | 56:888379912f81 | 73 | |
cocorlow | 56:888379912f81 | 74 | float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) |
cocorlow | 56:888379912f81 | 75 | { |
cocorlow | 56:888379912f81 | 76 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
NaotoMorita | 67:41fcdfb7cc5a | 77 | } |