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@61:c05353850017, 2021-06-03 (annotated)
- Committer:
- NaotoMorita
- Date:
- Thu Jun 03 11:28:13 2021 +0000
- Revision:
- 61:c05353850017
- Parent:
- 58:a7947322db87
- Child:
- 62:ef10fd919f7b
modify calibrate magneto
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 | |
cocorlow | 56:888379912f81 | 3 | // var |
cocorlow | 56:888379912f81 | 4 | |
cocorlow | 56:888379912f81 | 5 | // communication |
NaotoMorita | 58:a7947322db87 | 6 | Serial pc(USBTX, USBRX); |
cocorlow | 56:888379912f81 | 7 | I2C i2c(PB_9,PB_8); // sda, scl |
cocorlow | 56:888379912f81 | 8 | UsaPack tail(PG_14, PG_9, 57600); // log - tail |
cocorlow | 56:888379912f81 | 9 | SBUS sbus(PD_5, PD_6); |
cocorlow | 56:888379912f81 | 10 | |
cocorlow | 56:888379912f81 | 11 | // sensor |
cocorlow | 56:888379912f81 | 12 | MPU6050 accelgyro; |
cocorlow | 56:888379912f81 | 13 | MAG3110 mag_sensor(PB_9,PB_8); |
NaotoMorita | 61:c05353850017 | 14 | CalibrateMagneto magCalibrator; |
cocorlow | 56:888379912f81 | 15 | |
cocorlow | 56:888379912f81 | 16 | // io |
cocorlow | 56:888379912f81 | 17 | DigitalIn userButton(USER_BUTTON); |
cocorlow | 56:888379912f81 | 18 | DigitalIn locdef1(PG_2); |
cocorlow | 56:888379912f81 | 19 | DigitalIn locdef2(PG_3); |
cocorlow | 56:888379912f81 | 20 | |
cocorlow | 56:888379912f81 | 21 | // control |
cocorlow | 56:888379912f81 | 22 | FastPWM servo(PE_9); |
cocorlow | 56:888379912f81 | 23 | PID pitchPID(5.0, 0,0, PID_dt); // rad |
cocorlow | 56:888379912f81 | 24 | PID pitchratePID(0.5, 0.0, 0.0, PID_dt); // rad/s |
cocorlow | 56:888379912f81 | 25 | HAPS_EKF ekf; // EKF class |
cocorlow | 56:888379912f81 | 26 | |
cocorlow | 56:888379912f81 | 27 | int loop_count = 0; |
cocorlow | 56:888379912f81 | 28 | int obs_count = 0; |
cocorlow | 56:888379912f81 | 29 | float att_dt = 0.01f; |
cocorlow | 56:888379912f81 | 30 | float rc[16]; |
cocorlow | 56:888379912f81 | 31 | |
cocorlow | 56:888379912f81 | 32 | int16_t ax, ay, az; |
cocorlow | 56:888379912f81 | 33 | int16_t gx, gy, gz; |
cocorlow | 56:888379912f81 | 34 | MotionSensorDataUnits mdata; |
cocorlow | 56:888379912f81 | 35 | float magval[3] = {1.0f, 0.0f, 0.0f}; |
cocorlow | 56:888379912f81 | 36 | |
cocorlow | 56:888379912f81 | 37 | // position |
cocorlow | 56:888379912f81 | 38 | Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll y:pitch z:yaw |
cocorlow | 56:888379912f81 | 39 | Vector3 rpy_g(0.0f, 0.0f, 0.0f); // x:roll y:pitch z:yaw |
cocorlow | 56:888379912f81 | 40 | Vector3 acc; |
cocorlow | 56:888379912f81 | 41 | Vector3 accref(0.0f, 0.0f, -0.98f); |
cocorlow | 56:888379912f81 | 42 | Vector3 mag; |
cocorlow | 56:888379912f81 | 43 | Vector3 magref(0.65f, 0.0f, 0.75f); |
cocorlow | 56:888379912f81 | 44 | Vector3 dynacc; |
cocorlow | 56:888379912f81 | 45 | Vector3 gyro; |
cocorlow | 56:888379912f81 | 46 | |
cocorlow | 56:888379912f81 | 47 | Vector3 LPacc(0.0f, 0.0f, 0.0f); |
cocorlow | 56:888379912f81 | 48 | Vector3 LPmag(0.0f, 0.0f, 0.0f); |
cocorlow | 56:888379912f81 | 49 | |
cocorlow | 56:888379912f81 | 50 | int out1, out2; |
cocorlow | 56:888379912f81 | 51 | float scaledServoOut[1] = {0.0f}; |
cocorlow | 56:888379912f81 | 52 | float servoOut[1] = {1500.0f}; |
cocorlow | 56:888379912f81 | 53 | |
cocorlow | 56:888379912f81 | 54 | float val_thmg = 0.0f; |
NaotoMorita | 61:c05353850017 | 55 | float sigma_thmg = 0.0f; |
cocorlow | 56:888379912f81 | 56 | float th_mg = 0.0f; |
cocorlow | 56:888379912f81 | 57 | float accnormerr = 0.0f; |
NaotoMorita | 61:c05353850017 | 58 | float sigma_accnorm = 0.0f; |
cocorlow | 56:888379912f81 | 59 | |
cocorlow | 56:888379912f81 | 60 | int calibrationFlag = 0; |
cocorlow | 56:888379912f81 | 61 | int pos_tail = 0; // 0:left 1:center 2:right |
cocorlow | 56:888379912f81 | 62 | int agoffset[6] = {0, 0, 0, 386, -450, 48}; |
NaotoMorita | 61:c05353850017 | 63 | float magbiasMin[3] = {0.0f, 0.0f, 0.0f}; |
NaotoMorita | 61:c05353850017 | 64 | float magbiasMax[3] = {0.0f, 0.0f, 0.0f}; |
cocorlow | 56:888379912f81 | 65 | |
cocorlow | 56:888379912f81 | 66 | Vector3 rpy_align(0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f, 0.0f); |
cocorlow | 56:888379912f81 | 67 | |
cocorlow | 56:888379912f81 | 68 | // eepromのread writeのためのunionを定義 |
cocorlow | 56:888379912f81 | 69 | const int eeprom_address = 0xAE; // EEPROMの3つの入力が全て+より |
cocorlow | 56:888379912f81 | 70 | const int eeprom_pointeraddress = 0; |
cocorlow | 56:888379912f81 | 71 | |
cocorlow | 56:888379912f81 | 72 | // UsaPack |
cocorlow | 56:888379912f81 | 73 | const int tail_address[3] = {1111, 2222, 3333}; |
cocorlow | 56:888379912f81 | 74 | valuePack posValues[3]; |
cocorlow | 56:888379912f81 | 75 | |
cocorlow | 56:888379912f81 | 76 | float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) |
cocorlow | 56:888379912f81 | 77 | { |
cocorlow | 56:888379912f81 | 78 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cocorlow | 56:888379912f81 | 79 | } |