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@67:41fcdfb7cc5a, 2021-06-22 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Jun 22 02:45:43 2021 +0000
- Revision:
- 67:41fcdfb7cc5a
- Parent:
- 63:9c4973a98600
- Child:
- 68:b9f6938fab9d
compiled
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 | 63:9c4973a98600 | 17 | CalibrateMagneto joyCalibrator; |
cocorlow | 56:888379912f81 | 18 | // control |
NaotoMorita | 63:9c4973a98600 | 19 | FastPWM elevServo(PE_11); |
NaotoMorita | 63:9c4973a98600 | 20 | FastPWM rudServo(PE_13); |
cocorlow | 56:888379912f81 | 21 | PID pitchPID(5.0, 0,0, PID_dt); // rad |
cocorlow | 56:888379912f81 | 22 | PID pitchratePID(0.5, 0.0, 0.0, PID_dt); // rad/s |
NaotoMorita | 67:41fcdfb7cc5a | 23 | errStateEKF ekf; // EKF class |
cocorlow | 56:888379912f81 | 24 | |
cocorlow | 56:888379912f81 | 25 | int loop_count = 0; |
cocorlow | 56:888379912f81 | 26 | float att_dt = 0.01f; |
cocorlow | 56:888379912f81 | 27 | |
cocorlow | 56:888379912f81 | 28 | int16_t ax, ay, az; |
cocorlow | 56:888379912f81 | 29 | int16_t gx, gy, gz; |
cocorlow | 56:888379912f81 | 30 | MotionSensorDataUnits mdata; |
cocorlow | 56:888379912f81 | 31 | float magval[3] = {1.0f, 0.0f, 0.0f}; |
cocorlow | 56:888379912f81 | 32 | |
cocorlow | 56:888379912f81 | 33 | // position |
cocorlow | 56:888379912f81 | 34 | Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll y:pitch z:yaw |
cocorlow | 56:888379912f81 | 35 | Vector3 acc; |
cocorlow | 56:888379912f81 | 36 | Vector3 accref(0.0f, 0.0f, -0.98f); |
cocorlow | 56:888379912f81 | 37 | Vector3 mag; |
cocorlow | 56:888379912f81 | 38 | Vector3 magref(0.65f, 0.0f, 0.75f); |
cocorlow | 56:888379912f81 | 39 | Vector3 gyro; |
cocorlow | 56:888379912f81 | 40 | |
NaotoMorita | 63:9c4973a98600 | 41 | float scaledServoOut[2] = {0.0f,0.0f}; |
NaotoMorita | 63:9c4973a98600 | 42 | float servoOut[2] = {1500.0f,1500.0f}; |
cocorlow | 56:888379912f81 | 43 | |
cocorlow | 56:888379912f81 | 44 | int calibrationFlag = 0; |
cocorlow | 56:888379912f81 | 45 | int agoffset[6] = {0, 0, 0, 386, -450, 48}; |
NaotoMorita | 61:c05353850017 | 46 | float magbiasMin[3] = {0.0f, 0.0f, 0.0f}; |
NaotoMorita | 61:c05353850017 | 47 | float magbiasMax[3] = {0.0f, 0.0f, 0.0f}; |
cocorlow | 56:888379912f81 | 48 | |
NaotoMorita | 63:9c4973a98600 | 49 | Vector3 rpy_align(0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f); |
cocorlow | 56:888379912f81 | 50 | |
cocorlow | 56:888379912f81 | 51 | |
cocorlow | 56:888379912f81 | 52 | // UsaPack |
NaotoMorita | 63:9c4973a98600 | 53 | valuePack posValues; |
cocorlow | 56:888379912f81 | 54 | |
cocorlow | 56:888379912f81 | 55 | float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) |
cocorlow | 56:888379912f81 | 56 | { |
cocorlow | 56:888379912f81 | 57 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
NaotoMorita | 67:41fcdfb7cc5a | 58 | } |