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
global.hpp@135:49f8916588da, 2021-11-30 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Nov 30 12:04:58 2021 +0000
- Revision:
- 135:49f8916588da
- Parent:
- 134:d57c6b2a706b
- Child:
- 137:240588882ae2
preflight check
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 | |
cocorlow | 56:888379912f81 | 4 | #include "mbed.h" |
NaotoMorita | 70:99f974d8960e | 5 | #include "SBUS.hpp" |
cocorlow | 56:888379912f81 | 6 | #include "PIDcontroller.h" |
cocorlow | 56:888379912f81 | 7 | #include "LoopTicker.hpp" |
cocorlow | 56:888379912f81 | 8 | #include "FastPWM.h" |
cocorlow | 56:888379912f81 | 9 | #include <cmath> |
cocorlow | 56:888379912f81 | 10 | #include "UsaPack.hpp" |
cocorlow | 56:888379912f81 | 11 | #include "Vector3.hpp" |
osaka | 87:89bbbcdb667b | 12 | #include "Matrix.h" |
osaka | 87:89bbbcdb667b | 13 | #include "MatrixMath.h" |
osaka | 88:be349faa1976 | 14 | #include "LSM9DS1.h" |
osaka | 88:be349faa1976 | 15 | #include "LPS.h" |
NaotoMorita | 119:a21e283730d1 | 16 | #include "CalibrateMagneto.h" |
NaotoMorita | 90:96c2b0ed4b96 | 17 | #include "solaESKF.hpp" |
NaotoMorita | 70:99f974d8960e | 18 | #include "MedianFilter.hpp" |
osaka | 96:3645f8f9bdd3 | 19 | #include "GPSUBX_UART.hpp" |
osaka | 109:eb255fc4462b | 20 | #include "Autopilot.hpp" |
cocorlow | 56:888379912f81 | 21 | |
NaotoMorita | 104:20b8caa29185 | 22 | #define magresThreshold 0.025f |
cocorlow | 56:888379912f81 | 23 | #define M_PI 3.141592f |
cocorlow | 56:888379912f81 | 24 | #define ACCEL_SSF 4096.0f |
NaotoMorita | 70:99f974d8960e | 25 | #define GYRO_SSF 131.0f |
cocorlow | 56:888379912f81 | 26 | #define PID_dt 0.015f |
cocorlow | 56:888379912f81 | 27 | #define servoPwmMax 1800.0f |
cocorlow | 56:888379912f81 | 28 | #define servoPwmMin 1200.0f |
cocorlow | 56:888379912f81 | 29 | #define motorPwmMax 2000.0f |
cocorlow | 56:888379912f81 | 30 | #define motorPwmMin 1100.0f |
cocorlow | 56:888379912f81 | 31 | |
osaka | 88:be349faa1976 | 32 | |
cocorlow | 56:888379912f81 | 33 | struct valuePack |
cocorlow | 56:888379912f81 | 34 | { |
NaotoMorita | 73:84ffa0166e6c | 35 | int16_t accData[3]; |
NaotoMorita | 73:84ffa0166e6c | 36 | int16_t gyroData[3]; |
NaotoMorita | 84:ff48e01ea76b | 37 | int16_t magData[3]; |
NaotoMorita | 84:ff48e01ea76b | 38 | int16_t viData[3]; |
NaotoMorita | 92:00460f6df439 | 39 | int16_t piData[3]; |
NaotoMorita | 84:ff48e01ea76b | 40 | int16_t actData[4]; |
NaotoMorita | 76:7fd3ac1afe3e | 41 | int16_t commandIndex; |
NaotoMorita | 83:e69ab831031c | 42 | int16_t commandVal; |
NaotoMorita | 83:e69ab831031c | 43 | }; |
NaotoMorita | 83:e69ab831031c | 44 | |
NaotoMorita | 83:e69ab831031c | 45 | struct sendPack |
NaotoMorita | 83:e69ab831031c | 46 | { |
NaotoMorita | 83:e69ab831031c | 47 | float da; |
NaotoMorita | 83:e69ab831031c | 48 | float de; |
NaotoMorita | 83:e69ab831031c | 49 | float dT; |
NaotoMorita | 83:e69ab831031c | 50 | float rpy[3]; |
NaotoMorita | 84:ff48e01ea76b | 51 | float vihat[3]; |
cocorlow | 56:888379912f81 | 52 | }; |
NaotoMorita | 132:896ad37b534b | 53 | struct logPack |
NaotoMorita | 132:896ad37b534b | 54 | { |
NaotoMorita | 132:896ad37b534b | 55 | float time; |
NaotoMorita | 132:896ad37b534b | 56 | float hertz; |
NaotoMorita | 135:49f8916588da | 57 | float gpsFix; |
NaotoMorita | 132:896ad37b534b | 58 | float da; |
NaotoMorita | 132:896ad37b534b | 59 | float de; |
NaotoMorita | 132:896ad37b534b | 60 | float dT; |
NaotoMorita | 132:896ad37b534b | 61 | float rpy[3]; |
NaotoMorita | 132:896ad37b534b | 62 | float pihat[3]; |
NaotoMorita | 132:896ad37b534b | 63 | float vihat[3]; |
NaotoMorita | 132:896ad37b534b | 64 | float pi[3]; |
NaotoMorita | 132:896ad37b534b | 65 | float vi[3]; |
NaotoMorita | 132:896ad37b534b | 66 | float palt; |
NaotoMorita | 132:896ad37b534b | 67 | float acc[3]; |
NaotoMorita | 132:896ad37b534b | 68 | float gyro[3]; |
NaotoMorita | 132:896ad37b534b | 69 | float mag[3]; |
NaotoMorita | 133:346ce20b3950 | 70 | float rc[16]; |
NaotoMorita | 132:896ad37b534b | 71 | }; |
cocorlow | 56:888379912f81 | 72 | |
NaotoMorita | 134:d57c6b2a706b | 73 | struct telemetryPack |
NaotoMorita | 134:d57c6b2a706b | 74 | { |
NaotoMorita | 134:d57c6b2a706b | 75 | float time; |
NaotoMorita | 134:d57c6b2a706b | 76 | float hertz; |
NaotoMorita | 134:d57c6b2a706b | 77 | float gpsFix; |
NaotoMorita | 134:d57c6b2a706b | 78 | float rpy[3]; |
NaotoMorita | 134:d57c6b2a706b | 79 | float pihat[3]; |
NaotoMorita | 134:d57c6b2a706b | 80 | float vihat[3]; |
NaotoMorita | 134:d57c6b2a706b | 81 | float dynaccNorm; |
NaotoMorita | 134:d57c6b2a706b | 82 | }; |
NaotoMorita | 134:d57c6b2a706b | 83 | |
cocorlow | 56:888379912f81 | 84 | // var |
cocorlow | 56:888379912f81 | 85 | |
cocorlow | 56:888379912f81 | 86 | // communication |
osaka | 88:be349faa1976 | 87 | |
NaotoMorita | 64:e9661430f0e3 | 88 | extern UsaPack pc; // log - tail |
NaotoMorita | 132:896ad37b534b | 89 | extern UsaPack sd; |
NaotoMorita | 133:346ce20b3950 | 90 | extern UsaPack twelite; |
cocorlow | 56:888379912f81 | 91 | |
cocorlow | 56:888379912f81 | 92 | // io |
cocorlow | 56:888379912f81 | 93 | extern DigitalIn userButton; |
NaotoMorita | 70:99f974d8960e | 94 | extern SBUS sbus; |
osaka | 88:be349faa1976 | 95 | extern I2C i2c; // sda, scl |
osaka | 88:be349faa1976 | 96 | extern LSM9DS1 lsm; |
osaka | 88:be349faa1976 | 97 | extern LPS lps; |
osaka | 96:3645f8f9bdd3 | 98 | extern GPSUBX_UART gps; |
NaotoMorita | 119:a21e283730d1 | 99 | extern CalibrateMagneto magCalibrator; |
NaotoMorita | 102:1c77ff6e2a85 | 100 | extern float magres; |
cocorlow | 56:888379912f81 | 101 | // control |
NaotoMorita | 94:579e875a4244 | 102 | extern Timer _t; |
NaotoMorita | 70:99f974d8960e | 103 | extern FastPWM servoRight; |
NaotoMorita | 70:99f974d8960e | 104 | extern FastPWM servoLeft; |
NaotoMorita | 70:99f974d8960e | 105 | extern FastPWM servoThrust; |
NaotoMorita | 70:99f974d8960e | 106 | extern PID pitchPID; //rad |
NaotoMorita | 70:99f974d8960e | 107 | extern PID pitchratePID;//rad/s |
NaotoMorita | 70:99f974d8960e | 108 | extern PID rollPID; |
NaotoMorita | 70:99f974d8960e | 109 | extern PID rollratePID;//rad/s |
NaotoMorita | 90:96c2b0ed4b96 | 110 | extern solaESKF eskf; // EKF class |
NaotoMorita | 76:7fd3ac1afe3e | 111 | extern int obsCount; |
osaka | 109:eb255fc4462b | 112 | extern Autopilot autopilot; |
osaka | 109:eb255fc4462b | 113 | extern float roll_obj; |
osaka | 109:eb255fc4462b | 114 | extern float pitch_obj; |
osaka | 121:2523eef96b36 | 115 | extern float dT_obj; |
cocorlow | 56:888379912f81 | 116 | |
NaotoMorita | 70:99f974d8960e | 117 | extern float rc[16]; |
cocorlow | 56:888379912f81 | 118 | extern int loop_count; |
cocorlow | 56:888379912f81 | 119 | extern float att_dt; |
NaotoMorita | 112:6a33ea9f6fed | 120 | extern bool accmagSwitch; |
cocorlow | 56:888379912f81 | 121 | |
NaotoMorita | 70:99f974d8960e | 122 | |
cocorlow | 56:888379912f81 | 123 | // position |
NaotoMorita | 113:3e47d9881529 | 124 | extern Matrix SensorAlignmentAG; |
NaotoMorita | 113:3e47d9881529 | 125 | extern Matrix SensorAlignmentMAG; |
NaotoMorita | 132:896ad37b534b | 126 | extern Matrix euler; |
cocorlow | 56:888379912f81 | 127 | extern Vector3 rpy; // x:roll y:pitch z:yaw |
cocorlow | 56:888379912f81 | 128 | extern Vector3 acc; |
cocorlow | 56:888379912f81 | 129 | extern Vector3 accref; |
cocorlow | 56:888379912f81 | 130 | extern Vector3 mag; |
cocorlow | 56:888379912f81 | 131 | extern Vector3 magref; |
cocorlow | 56:888379912f81 | 132 | extern Vector3 gyro; |
NaotoMorita | 82:c183c29d2427 | 133 | extern Vector3 vi; |
NaotoMorita | 92:00460f6df439 | 134 | extern Vector3 pi; |
NaotoMorita | 92:00460f6df439 | 135 | extern float palt; |
NaotoMorita | 98:bdaa6bbfb1d9 | 136 | extern float palt0; |
NaotoMorita | 129:a76be8380bb2 | 137 | extern float dynaccnorm2; |
NaotoMorita | 106:2d854e92cebb | 138 | extern int itow_status; |
NaotoMorita | 100:7589b663d462 | 139 | extern bool gpsUpdateFlag; |
NaotoMorita | 106:2d854e92cebb | 140 | extern bool gpsLlh0Fixed; |
NaotoMorita | 127:d73a6233ee4b | 141 | extern bool headingUpdateFlag; |
osaka | 87:89bbbcdb667b | 142 | |
NaotoMorita | 73:84ffa0166e6c | 143 | extern float de; |
NaotoMorita | 73:84ffa0166e6c | 144 | extern float da; |
NaotoMorita | 73:84ffa0166e6c | 145 | extern float dT; |
NaotoMorita | 70:99f974d8960e | 146 | extern MedianFilter accMedian; |
NaotoMorita | 70:99f974d8960e | 147 | extern MedianFilter gyroMedian; |
NaotoMorita | 70:99f974d8960e | 148 | extern MedianFilter magMedian; |
cocorlow | 56:888379912f81 | 149 | |
NaotoMorita | 64:e9661430f0e3 | 150 | extern float scaledServoOut[2]; |
NaotoMorita | 70:99f974d8960e | 151 | extern float scaledMotorOut[1]; |
NaotoMorita | 64:e9661430f0e3 | 152 | extern float servoOut[2]; |
NaotoMorita | 70:99f974d8960e | 153 | extern float motorOut[1]; |
cocorlow | 56:888379912f81 | 154 | |
NaotoMorita | 61:c05353850017 | 155 | |
cocorlow | 56:888379912f81 | 156 | extern int calibrationFlag; |
NaotoMorita | 93:b827f78a717a | 157 | extern float agoffset[6]; |
NaotoMorita | 61:c05353850017 | 158 | extern float magbiasMin[3]; |
NaotoMorita | 61:c05353850017 | 159 | extern float magbiasMax[3]; |
NaotoMorita | 76:7fd3ac1afe3e | 160 | extern float magbias[4]; |
NaotoMorita | 70:99f974d8960e | 161 | extern float accMin[3]; |
NaotoMorita | 70:99f974d8960e | 162 | extern float accMax[3]; |
cocorlow | 56:888379912f81 | 163 | |
cocorlow | 56:888379912f81 | 164 | extern Vector3 rpy_align; |
cocorlow | 56:888379912f81 | 165 | |
cocorlow | 56:888379912f81 | 166 | |
cocorlow | 56:888379912f81 | 167 | //// UsaPack |
NaotoMorita | 73:84ffa0166e6c | 168 | extern valuePack vp; |
NaotoMorita | 83:e69ab831031c | 169 | extern sendPack sp; |
NaotoMorita | 132:896ad37b534b | 170 | extern logPack lp; |
NaotoMorita | 134:d57c6b2a706b | 171 | extern telemetryPack tp; |
NaotoMorita | 73:84ffa0166e6c | 172 | |
NaotoMorita | 73:84ffa0166e6c | 173 | // HIL |
NaotoMorita | 73:84ffa0166e6c | 174 | extern bool hilFlag; |
NaotoMorita | 76:7fd3ac1afe3e | 175 | extern bool serialControlSource; |
NaotoMorita | 76:7fd3ac1afe3e | 176 | extern bool serialParamSource; |
NaotoMorita | 76:7fd3ac1afe3e | 177 | extern int checkParamSerial[5]; |
cocorlow | 56:888379912f81 | 178 | // function |
cocorlow | 56:888379912f81 | 179 | |
cocorlow | 56:888379912f81 | 180 | // main.cpp |
cocorlow | 56:888379912f81 | 181 | |
cocorlow | 56:888379912f81 | 182 | // setup.cpp |
cocorlow | 56:888379912f81 | 183 | extern void setup(); |
cocorlow | 56:888379912f81 | 184 | extern void calibrate(); |
cocorlow | 56:888379912f81 | 185 | // run.cpp |
cocorlow | 56:888379912f81 | 186 | extern void run(); |
cocorlow | 56:888379912f81 | 187 | |
cocorlow | 56:888379912f81 | 188 | // imu.cpp |
osaka | 88:be349faa1976 | 189 | extern void getIMUval(); |
cocorlow | 56:888379912f81 | 190 | |
osaka | 96:3645f8f9bdd3 | 191 | //gps.cpp |
osaka | 96:3645f8f9bdd3 | 192 | extern void getGPSval(); |
osaka | 97:2b3242c2dd85 | 193 | extern void change_refpos(); |
osaka | 96:3645f8f9bdd3 | 194 | |
NaotoMorita | 73:84ffa0166e6c | 195 | // hil.cpp |
NaotoMorita | 92:00460f6df439 | 196 | extern void getHilIMUval(); |
NaotoMorita | 92:00460f6df439 | 197 | extern void getHilGPSval(); |
NaotoMorita | 73:84ffa0166e6c | 198 | extern float randn(); |
NaotoMorita | 73:84ffa0166e6c | 199 | |
osaka | 109:eb255fc4462b | 200 | //autopilot.cpp |
osaka | 109:eb255fc4462b | 201 | extern void level_flight(); |
osaka | 109:eb255fc4462b | 202 | extern void point_guide(); |
osaka | 109:eb255fc4462b | 203 | extern void turning(); |
osaka | 109:eb255fc4462b | 204 | |
cocorlow | 56:888379912f81 | 205 | // servo.cpp |
cocorlow | 56:888379912f81 | 206 | extern void calcServoOut(); |
cocorlow | 56:888379912f81 | 207 | |
osaka | 87:89bbbcdb667b | 208 | // transferData.cpp |
osaka | 87:89bbbcdb667b | 209 | extern void sendData2PC(); |
osaka | 87:89bbbcdb667b | 210 | extern void sendTelemetry(); |
osaka | 87:89bbbcdb667b | 211 | extern void writeSDcard(); |
cocorlow | 56:888379912f81 | 212 | |
cocorlow | 56:888379912f81 | 213 | // global.cpp |
cocorlow | 56:888379912f81 | 214 | extern float mapfloat(float x, float in_min, float in_max, float out_min, float out_max); |
NaotoMorita | 93:b827f78a717a | 215 | extern void setDiag(Matrix& mat, float val); |
NaotoMorita | 93:b827f78a717a | 216 | extern void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex); |
cocorlow | 56:888379912f81 | 217 | #endif |