solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
global.hpp@88:be349faa1976, 2021-10-21 (annotated)
- Committer:
- osaka
- Date:
- Thu Oct 21 06:41:03 2021 +0000
- Revision:
- 88:be349faa1976
- Parent:
- 87:89bbbcdb667b
- Child:
- 89:c9f64bd655d9
newer
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 56:888379912f81 | 1 | #ifndef __GLOBAL_HPP__ |
cocorlow | 56:888379912f81 | 2 | #define __GLOBAL_HPP__ |
cocorlow | 56:888379912f81 | 3 | |
osaka | 88:be349faa1976 | 4 | using namespace std; |
osaka | 88:be349faa1976 | 5 | |
cocorlow | 56:888379912f81 | 6 | #include "mbed.h" |
NaotoMorita | 70:99f974d8960e | 7 | #include "SBUS.hpp" |
cocorlow | 56:888379912f81 | 8 | #include "PIDcontroller.h" |
cocorlow | 56:888379912f81 | 9 | #include "LoopTicker.hpp" |
cocorlow | 56:888379912f81 | 10 | #include "FastPWM.h" |
cocorlow | 56:888379912f81 | 11 | #include <cmath> |
cocorlow | 56:888379912f81 | 12 | #include "UsaPack.hpp" |
cocorlow | 56:888379912f81 | 13 | #include "Vector3.hpp" |
osaka | 87:89bbbcdb667b | 14 | #include "Matrix.h" |
osaka | 87:89bbbcdb667b | 15 | #include "MatrixMath.h" |
osaka | 88:be349faa1976 | 16 | #include "LSM9DS1.h" |
osaka | 88:be349faa1976 | 17 | #include "LPS.h" |
NaotoMorita | 76:7fd3ac1afe3e | 18 | #include "ScErrStateEKF.hpp" |
NaotoMorita | 70:99f974d8960e | 19 | #include "MedianFilter.hpp" |
cocorlow | 56:888379912f81 | 20 | |
cocorlow | 56:888379912f81 | 21 | #define M_PI 3.141592f |
cocorlow | 56:888379912f81 | 22 | #define ACCEL_SSF 4096.0f |
NaotoMorita | 70:99f974d8960e | 23 | #define GYRO_SSF 131.0f |
cocorlow | 56:888379912f81 | 24 | #define PID_dt 0.015f |
cocorlow | 56:888379912f81 | 25 | #define servoPwmMax 1800.0f |
cocorlow | 56:888379912f81 | 26 | #define servoPwmMin 1200.0f |
cocorlow | 56:888379912f81 | 27 | #define motorPwmMax 2000.0f |
cocorlow | 56:888379912f81 | 28 | #define motorPwmMin 1100.0f |
cocorlow | 56:888379912f81 | 29 | |
osaka | 88:be349faa1976 | 30 | |
cocorlow | 56:888379912f81 | 31 | struct valuePack |
cocorlow | 56:888379912f81 | 32 | { |
NaotoMorita | 73:84ffa0166e6c | 33 | int16_t accData[3]; |
NaotoMorita | 73:84ffa0166e6c | 34 | int16_t gyroData[3]; |
NaotoMorita | 84:ff48e01ea76b | 35 | int16_t magData[3]; |
NaotoMorita | 84:ff48e01ea76b | 36 | int16_t viData[3]; |
NaotoMorita | 84:ff48e01ea76b | 37 | int16_t actData[4]; |
NaotoMorita | 76:7fd3ac1afe3e | 38 | int16_t commandIndex; |
NaotoMorita | 83:e69ab831031c | 39 | int16_t commandVal; |
NaotoMorita | 83:e69ab831031c | 40 | }; |
NaotoMorita | 83:e69ab831031c | 41 | |
NaotoMorita | 83:e69ab831031c | 42 | struct sendPack |
NaotoMorita | 83:e69ab831031c | 43 | { |
NaotoMorita | 83:e69ab831031c | 44 | float da; |
NaotoMorita | 83:e69ab831031c | 45 | float de; |
NaotoMorita | 83:e69ab831031c | 46 | float dT; |
NaotoMorita | 83:e69ab831031c | 47 | float rpy[3]; |
NaotoMorita | 84:ff48e01ea76b | 48 | float vi[3]; |
NaotoMorita | 84:ff48e01ea76b | 49 | float vihat[3]; |
osaka | 86:456f00d52974 | 50 | float accBias[3]; |
cocorlow | 56:888379912f81 | 51 | }; |
cocorlow | 56:888379912f81 | 52 | |
cocorlow | 56:888379912f81 | 53 | // var |
cocorlow | 56:888379912f81 | 54 | |
cocorlow | 56:888379912f81 | 55 | // communication |
osaka | 88:be349faa1976 | 56 | |
NaotoMorita | 64:e9661430f0e3 | 57 | extern UsaPack pc; // log - tail |
NaotoMorita | 76:7fd3ac1afe3e | 58 | extern Serial sd; |
osaka | 87:89bbbcdb667b | 59 | extern Serial twelite; |
cocorlow | 56:888379912f81 | 60 | |
cocorlow | 56:888379912f81 | 61 | // io |
cocorlow | 56:888379912f81 | 62 | extern DigitalIn userButton; |
NaotoMorita | 70:99f974d8960e | 63 | extern SBUS sbus; |
osaka | 88:be349faa1976 | 64 | extern I2C i2c; // sda, scl |
osaka | 88:be349faa1976 | 65 | extern LSM9DS1 lsm; |
osaka | 88:be349faa1976 | 66 | extern LPS lps; |
cocorlow | 56:888379912f81 | 67 | |
cocorlow | 56:888379912f81 | 68 | // control |
NaotoMorita | 70:99f974d8960e | 69 | extern FastPWM servoRight; |
NaotoMorita | 70:99f974d8960e | 70 | extern FastPWM servoLeft; |
NaotoMorita | 70:99f974d8960e | 71 | extern FastPWM servoThrust; |
NaotoMorita | 70:99f974d8960e | 72 | extern PID pitchPID; //rad |
NaotoMorita | 70:99f974d8960e | 73 | extern PID pitchratePID;//rad/s |
NaotoMorita | 70:99f974d8960e | 74 | extern PID rollPID; |
NaotoMorita | 70:99f974d8960e | 75 | extern PID rollratePID;//rad/s |
NaotoMorita | 76:7fd3ac1afe3e | 76 | extern ScErrStateEKF ekf; // EKF class |
NaotoMorita | 76:7fd3ac1afe3e | 77 | extern int obsCount; |
cocorlow | 56:888379912f81 | 78 | |
NaotoMorita | 70:99f974d8960e | 79 | extern float rc[16]; |
cocorlow | 56:888379912f81 | 80 | extern int loop_count; |
cocorlow | 56:888379912f81 | 81 | extern float att_dt; |
cocorlow | 56:888379912f81 | 82 | |
NaotoMorita | 70:99f974d8960e | 83 | |
cocorlow | 56:888379912f81 | 84 | // position |
cocorlow | 56:888379912f81 | 85 | extern Vector3 rpy; // x:roll y:pitch z:yaw |
cocorlow | 56:888379912f81 | 86 | extern Vector3 acc; |
cocorlow | 56:888379912f81 | 87 | extern Vector3 accref; |
cocorlow | 56:888379912f81 | 88 | extern Vector3 mag; |
cocorlow | 56:888379912f81 | 89 | extern Vector3 magref; |
cocorlow | 56:888379912f81 | 90 | extern Vector3 gyro; |
NaotoMorita | 82:c183c29d2427 | 91 | extern Vector3 vi; |
osaka | 87:89bbbcdb667b | 92 | |
NaotoMorita | 73:84ffa0166e6c | 93 | extern float de; |
NaotoMorita | 73:84ffa0166e6c | 94 | extern float da; |
NaotoMorita | 73:84ffa0166e6c | 95 | extern float dT; |
NaotoMorita | 70:99f974d8960e | 96 | extern MedianFilter accMedian; |
NaotoMorita | 70:99f974d8960e | 97 | extern MedianFilter gyroMedian; |
NaotoMorita | 70:99f974d8960e | 98 | extern MedianFilter magMedian; |
cocorlow | 56:888379912f81 | 99 | |
NaotoMorita | 64:e9661430f0e3 | 100 | extern float scaledServoOut[2]; |
NaotoMorita | 70:99f974d8960e | 101 | extern float scaledMotorOut[1]; |
NaotoMorita | 64:e9661430f0e3 | 102 | extern float servoOut[2]; |
NaotoMorita | 70:99f974d8960e | 103 | extern float motorOut[1]; |
cocorlow | 56:888379912f81 | 104 | |
NaotoMorita | 61:c05353850017 | 105 | |
cocorlow | 56:888379912f81 | 106 | extern int calibrationFlag; |
cocorlow | 56:888379912f81 | 107 | extern int agoffset[6]; |
NaotoMorita | 61:c05353850017 | 108 | extern float magbiasMin[3]; |
NaotoMorita | 61:c05353850017 | 109 | extern float magbiasMax[3]; |
NaotoMorita | 76:7fd3ac1afe3e | 110 | extern float magbias[4]; |
NaotoMorita | 70:99f974d8960e | 111 | extern float accMin[3]; |
NaotoMorita | 70:99f974d8960e | 112 | extern float accMax[3]; |
cocorlow | 56:888379912f81 | 113 | |
cocorlow | 56:888379912f81 | 114 | extern Vector3 rpy_align; |
cocorlow | 56:888379912f81 | 115 | |
cocorlow | 56:888379912f81 | 116 | |
cocorlow | 56:888379912f81 | 117 | //// UsaPack |
NaotoMorita | 73:84ffa0166e6c | 118 | extern valuePack vp; |
NaotoMorita | 83:e69ab831031c | 119 | extern sendPack sp; |
NaotoMorita | 73:84ffa0166e6c | 120 | |
NaotoMorita | 73:84ffa0166e6c | 121 | // HIL |
NaotoMorita | 73:84ffa0166e6c | 122 | extern bool hilFlag; |
NaotoMorita | 76:7fd3ac1afe3e | 123 | extern bool serialControlSource; |
NaotoMorita | 76:7fd3ac1afe3e | 124 | extern bool serialParamSource; |
NaotoMorita | 76:7fd3ac1afe3e | 125 | extern int checkParamSerial[5]; |
cocorlow | 56:888379912f81 | 126 | // function |
cocorlow | 56:888379912f81 | 127 | |
cocorlow | 56:888379912f81 | 128 | // main.cpp |
cocorlow | 56:888379912f81 | 129 | |
cocorlow | 56:888379912f81 | 130 | // setup.cpp |
cocorlow | 56:888379912f81 | 131 | extern void setup(); |
cocorlow | 56:888379912f81 | 132 | extern void calibrate(); |
cocorlow | 56:888379912f81 | 133 | // run.cpp |
cocorlow | 56:888379912f81 | 134 | extern void run(); |
cocorlow | 56:888379912f81 | 135 | |
cocorlow | 56:888379912f81 | 136 | // imu.cpp |
osaka | 88:be349faa1976 | 137 | extern void getIMUval(); |
cocorlow | 56:888379912f81 | 138 | |
NaotoMorita | 73:84ffa0166e6c | 139 | // hil.cpp |
NaotoMorita | 73:84ffa0166e6c | 140 | extern void getHILval(); |
NaotoMorita | 73:84ffa0166e6c | 141 | extern float randn(); |
NaotoMorita | 73:84ffa0166e6c | 142 | |
cocorlow | 56:888379912f81 | 143 | // servo.cpp |
cocorlow | 56:888379912f81 | 144 | extern void calcServoOut(); |
cocorlow | 56:888379912f81 | 145 | |
osaka | 87:89bbbcdb667b | 146 | // transferData.cpp |
osaka | 87:89bbbcdb667b | 147 | extern void sendData2PC(); |
osaka | 87:89bbbcdb667b | 148 | extern void sendTelemetry(); |
osaka | 87:89bbbcdb667b | 149 | extern void writeSDcard(); |
cocorlow | 56:888379912f81 | 150 | |
cocorlow | 56:888379912f81 | 151 | // global.cpp |
cocorlow | 56:888379912f81 | 152 | extern float mapfloat(float x, float in_min, float in_max, float out_min, float out_max); |
cocorlow | 56:888379912f81 | 153 | |
cocorlow | 56:888379912f81 | 154 | #endif |