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@144:b3a713b4f1c4, 2022-06-29 (annotated)
- Committer:
- NaotoMorita
- Date:
- Wed Jun 29 07:57:10 2022 +0000
- Revision:
- 144:b3a713b4f1c4
- Parent:
- 143:53808e4e684c
can fly
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" |
osaka | 88:be349faa1976 | 11 | #include "LSM9DS1.h" |
osaka | 88:be349faa1976 | 12 | #include "LPS.h" |
NaotoMorita | 119:a21e283730d1 | 13 | #include "CalibrateMagneto.h" |
NaotoMorita | 90:96c2b0ed4b96 | 14 | #include "solaESKF.hpp" |
osaka | 96:3645f8f9bdd3 | 15 | #include "GPSUBX_UART.hpp" |
cocorlow | 56:888379912f81 | 16 | |
cocorlow | 139:b378528c05f2 | 17 | // Eigen |
cocorlow | 139:b378528c05f2 | 18 | #include <Eigen/Dense.h> |
cocorlow | 141:725321fe2949 | 19 | //#include <Eigen/Core.h> |
cocorlow | 141:725321fe2949 | 20 | //#include <Eigen/LU.h> |
cocorlow | 141:725321fe2949 | 21 | //#include <Eigen/Geometry.h> |
cocorlow | 139:b378528c05f2 | 22 | //using namespace std; |
cocorlow | 139:b378528c05f2 | 23 | using namespace Eigen; |
cocorlow | 139:b378528c05f2 | 24 | |
NaotoMorita | 104:20b8caa29185 | 25 | #define magresThreshold 0.025f |
cocorlow | 56:888379912f81 | 26 | #define ACCEL_SSF 4096.0f |
NaotoMorita | 70:99f974d8960e | 27 | #define GYRO_SSF 131.0f |
cocorlow | 56:888379912f81 | 28 | #define PID_dt 0.015f |
cocorlow | 56:888379912f81 | 29 | #define servoPwmMax 1800.0f |
cocorlow | 56:888379912f81 | 30 | #define servoPwmMin 1200.0f |
cocorlow | 56:888379912f81 | 31 | #define motorPwmMax 2000.0f |
cocorlow | 56:888379912f81 | 32 | #define motorPwmMin 1100.0f |
cocorlow | 56:888379912f81 | 33 | |
cocorlow | 139:b378528c05f2 | 34 | #ifndef M_PI_F |
cocorlow | 139:b378528c05f2 | 35 | #define M_PI_F 3.141592f |
cocorlow | 139:b378528c05f2 | 36 | #endif |
osaka | 88:be349faa1976 | 37 | |
NaotoMorita | 143:53808e4e684c | 38 | |
cocorlow | 56:888379912f81 | 39 | struct valuePack |
cocorlow | 56:888379912f81 | 40 | { |
NaotoMorita | 73:84ffa0166e6c | 41 | int16_t accData[3]; |
NaotoMorita | 73:84ffa0166e6c | 42 | int16_t gyroData[3]; |
NaotoMorita | 84:ff48e01ea76b | 43 | int16_t magData[3]; |
NaotoMorita | 84:ff48e01ea76b | 44 | int16_t viData[3]; |
NaotoMorita | 92:00460f6df439 | 45 | int16_t piData[3]; |
NaotoMorita | 143:53808e4e684c | 46 | //int16_t airspeed; |
NaotoMorita | 143:53808e4e684c | 47 | //int16_t actData[4]; |
NaotoMorita | 76:7fd3ac1afe3e | 48 | int16_t commandIndex; |
NaotoMorita | 83:e69ab831031c | 49 | int16_t commandVal; |
NaotoMorita | 83:e69ab831031c | 50 | }; |
NaotoMorita | 83:e69ab831031c | 51 | |
NaotoMorita | 83:e69ab831031c | 52 | struct sendPack |
NaotoMorita | 83:e69ab831031c | 53 | { |
NaotoMorita | 144:b3a713b4f1c4 | 54 | float motor[6]; |
NaotoMorita | 143:53808e4e684c | 55 | float euler[3]; |
NaotoMorita | 143:53808e4e684c | 56 | float state[4]; |
NaotoMorita | 143:53808e4e684c | 57 | float variance[3]; |
NaotoMorita | 143:53808e4e684c | 58 | |
cocorlow | 56:888379912f81 | 59 | }; |
NaotoMorita | 132:896ad37b534b | 60 | struct logPack |
NaotoMorita | 132:896ad37b534b | 61 | { |
NaotoMorita | 143:53808e4e684c | 62 | float time; // 1 |
NaotoMorita | 143:53808e4e684c | 63 | float hertz; // 2 |
NaotoMorita | 143:53808e4e684c | 64 | float gpsFix; // 3 |
NaotoMorita | 143:53808e4e684c | 65 | float da; // 4 |
NaotoMorita | 143:53808e4e684c | 66 | float de; // 5 |
NaotoMorita | 143:53808e4e684c | 67 | float dT; // 6 |
NaotoMorita | 143:53808e4e684c | 68 | float rpy[3]; // 7 8 9 |
NaotoMorita | 143:53808e4e684c | 69 | float pihat[3]; // 10 11 12 |
NaotoMorita | 143:53808e4e684c | 70 | float vihat[3]; // 13 14 15 |
NaotoMorita | 143:53808e4e684c | 71 | float pi[3]; // 16 17 18 |
NaotoMorita | 143:53808e4e684c | 72 | float vi[3]; // 19 20 21 |
NaotoMorita | 143:53808e4e684c | 73 | float palt; // 22 |
NaotoMorita | 143:53808e4e684c | 74 | float acc[3]; // 23 24 25 |
NaotoMorita | 143:53808e4e684c | 75 | float gyro[3]; // 26 27 28 |
NaotoMorita | 143:53808e4e684c | 76 | float mag[3]; // 29 30 31 |
NaotoMorita | 143:53808e4e684c | 77 | float rc[16]; // 32 ... 47 |
NaotoMorita | 143:53808e4e684c | 78 | float wind[3]; // 48 49 50 |
NaotoMorita | 143:53808e4e684c | 79 | float pitch_obj; // 51 |
NaotoMorita | 143:53808e4e684c | 80 | float roll_obj; // 52 |
NaotoMorita | 143:53808e4e684c | 81 | float u_pitot; // 53 |
NaotoMorita | 132:896ad37b534b | 82 | }; |
cocorlow | 56:888379912f81 | 83 | |
NaotoMorita | 134:d57c6b2a706b | 84 | struct telemetryPack |
NaotoMorita | 134:d57c6b2a706b | 85 | { |
NaotoMorita | 143:53808e4e684c | 86 | float pi[3]; |
NaotoMorita | 143:53808e4e684c | 87 | float rpy_l[3]; |
NaotoMorita | 143:53808e4e684c | 88 | float rpy_c[3]; |
NaotoMorita | 143:53808e4e684c | 89 | float rpy_r[3]; |
NaotoMorita | 143:53808e4e684c | 90 | float vi[3]; |
NaotoMorita | 143:53808e4e684c | 91 | float palt; |
NaotoMorita | 143:53808e4e684c | 92 | float gps_fix; |
NaotoMorita | 143:53808e4e684c | 93 | float mode; //preflight:0, flight:1 |
NaotoMorita | 134:d57c6b2a706b | 94 | float time; |
NaotoMorita | 143:53808e4e684c | 95 | float gps_acc; |
NaotoMorita | 143:53808e4e684c | 96 | float vx_opt; |
NaotoMorita | 143:53808e4e684c | 97 | float vy_opt; |
NaotoMorita | 143:53808e4e684c | 98 | float dist_ir; |
NaotoMorita | 143:53808e4e684c | 99 | float voltage[2]; |
NaotoMorita | 143:53808e4e684c | 100 | float current[2]; |
NaotoMorita | 143:53808e4e684c | 101 | float wind[3]; |
NaotoMorita | 143:53808e4e684c | 102 | float pitch_obj; |
NaotoMorita | 143:53808e4e684c | 103 | float roll_obj; |
NaotoMorita | 143:53808e4e684c | 104 | float hinf; |
NaotoMorita | 143:53808e4e684c | 105 | float u_pitot; |
NaotoMorita | 134:d57c6b2a706b | 106 | }; |
NaotoMorita | 134:d57c6b2a706b | 107 | |
cocorlow | 56:888379912f81 | 108 | // var |
cocorlow | 56:888379912f81 | 109 | |
cocorlow | 56:888379912f81 | 110 | // communication |
osaka | 88:be349faa1976 | 111 | |
NaotoMorita | 64:e9661430f0e3 | 112 | extern UsaPack pc; // log - tail |
NaotoMorita | 132:896ad37b534b | 113 | extern UsaPack sd; |
NaotoMorita | 133:346ce20b3950 | 114 | extern UsaPack twelite; |
cocorlow | 56:888379912f81 | 115 | |
cocorlow | 56:888379912f81 | 116 | // io |
cocorlow | 56:888379912f81 | 117 | extern DigitalIn userButton; |
NaotoMorita | 70:99f974d8960e | 118 | extern SBUS sbus; |
osaka | 88:be349faa1976 | 119 | extern I2C i2c; // sda, scl |
osaka | 88:be349faa1976 | 120 | extern LSM9DS1 lsm; |
osaka | 88:be349faa1976 | 121 | extern LPS lps; |
osaka | 96:3645f8f9bdd3 | 122 | extern GPSUBX_UART gps; |
NaotoMorita | 119:a21e283730d1 | 123 | extern CalibrateMagneto magCalibrator; |
NaotoMorita | 143:53808e4e684c | 124 | |
cocorlow | 56:888379912f81 | 125 | // control |
NaotoMorita | 94:579e875a4244 | 126 | extern Timer _t; |
NaotoMorita | 144:b3a713b4f1c4 | 127 | extern FastPWM motor1; |
NaotoMorita | 144:b3a713b4f1c4 | 128 | extern FastPWM motor2; |
NaotoMorita | 144:b3a713b4f1c4 | 129 | extern FastPWM motor3; |
NaotoMorita | 144:b3a713b4f1c4 | 130 | extern FastPWM motor4; |
NaotoMorita | 144:b3a713b4f1c4 | 131 | extern FastPWM motor5; |
NaotoMorita | 144:b3a713b4f1c4 | 132 | extern FastPWM motor6; |
NaotoMorita | 70:99f974d8960e | 133 | extern PID pitchPID; //rad |
NaotoMorita | 70:99f974d8960e | 134 | extern PID pitchratePID;//rad/s |
NaotoMorita | 70:99f974d8960e | 135 | extern PID rollPID; |
NaotoMorita | 70:99f974d8960e | 136 | extern PID rollratePID;//rad/s |
NaotoMorita | 143:53808e4e684c | 137 | extern PID yawratePID;//rad/s |
NaotoMorita | 144:b3a713b4f1c4 | 138 | extern PID vxPID;//m/s |
NaotoMorita | 144:b3a713b4f1c4 | 139 | extern PID vyPID;//m/s |
NaotoMorita | 144:b3a713b4f1c4 | 140 | extern PID vzPID;//m/s |
NaotoMorita | 144:b3a713b4f1c4 | 141 | |
cocorlow | 140:53dbdb207542 | 142 | extern solaESKF eskf; // EKF class |
NaotoMorita | 144:b3a713b4f1c4 | 143 | |
cocorlow | 56:888379912f81 | 144 | |
NaotoMorita | 70:99f974d8960e | 145 | extern float rc[16]; |
cocorlow | 56:888379912f81 | 146 | extern int loop_count; |
cocorlow | 56:888379912f81 | 147 | extern float att_dt; |
cocorlow | 56:888379912f81 | 148 | |
NaotoMorita | 70:99f974d8960e | 149 | |
cocorlow | 56:888379912f81 | 150 | // position |
cocorlow | 139:b378528c05f2 | 151 | extern Matrix3f SensorAlignmentAG; |
cocorlow | 139:b378528c05f2 | 152 | extern Matrix3f SensorAlignmentMAG; |
cocorlow | 139:b378528c05f2 | 153 | extern Vector3f euler; |
cocorlow | 139:b378528c05f2 | 154 | extern Vector3f acc; |
cocorlow | 139:b378528c05f2 | 155 | extern Vector3f mag; |
cocorlow | 139:b378528c05f2 | 156 | extern Vector3f magref; |
cocorlow | 139:b378528c05f2 | 157 | extern Vector3f gyro; |
cocorlow | 139:b378528c05f2 | 158 | extern Vector3f vi; |
cocorlow | 139:b378528c05f2 | 159 | extern Vector3f pi; |
NaotoMorita | 92:00460f6df439 | 160 | extern float palt; |
NaotoMorita | 98:bdaa6bbfb1d9 | 161 | extern float palt0; |
NaotoMorita | 129:a76be8380bb2 | 162 | extern float dynaccnorm2; |
NaotoMorita | 106:2d854e92cebb | 163 | extern int itow_status; |
NaotoMorita | 100:7589b663d462 | 164 | extern bool gpsUpdateFlag; |
NaotoMorita | 106:2d854e92cebb | 165 | extern bool gpsLlh0Fixed; |
NaotoMorita | 127:d73a6233ee4b | 166 | extern bool headingUpdateFlag; |
NaotoMorita | 143:53808e4e684c | 167 | extern bool hinf_flag; |
osaka | 87:89bbbcdb667b | 168 | |
NaotoMorita | 73:84ffa0166e6c | 169 | extern float de; |
NaotoMorita | 73:84ffa0166e6c | 170 | extern float da; |
NaotoMorita | 144:b3a713b4f1c4 | 171 | extern float dr; |
NaotoMorita | 73:84ffa0166e6c | 172 | extern float dT; |
cocorlow | 56:888379912f81 | 173 | |
NaotoMorita | 144:b3a713b4f1c4 | 174 | extern float scaledMotorOut[6]; |
NaotoMorita | 144:b3a713b4f1c4 | 175 | extern float motorOut[6]; |
cocorlow | 56:888379912f81 | 176 | |
NaotoMorita | 93:b827f78a717a | 177 | extern float agoffset[6]; |
cocorlow | 56:888379912f81 | 178 | |
cocorlow | 56:888379912f81 | 179 | |
cocorlow | 56:888379912f81 | 180 | //// UsaPack |
NaotoMorita | 73:84ffa0166e6c | 181 | extern valuePack vp; |
NaotoMorita | 83:e69ab831031c | 182 | extern sendPack sp; |
NaotoMorita | 132:896ad37b534b | 183 | extern logPack lp; |
NaotoMorita | 134:d57c6b2a706b | 184 | extern telemetryPack tp; |
NaotoMorita | 73:84ffa0166e6c | 185 | |
NaotoMorita | 73:84ffa0166e6c | 186 | // HIL |
NaotoMorita | 73:84ffa0166e6c | 187 | extern bool hilFlag; |
NaotoMorita | 143:53808e4e684c | 188 | extern int16_t hilDataOut; |
NaotoMorita | 143:53808e4e684c | 189 | |
cocorlow | 56:888379912f81 | 190 | // function |
cocorlow | 56:888379912f81 | 191 | |
cocorlow | 56:888379912f81 | 192 | // main.cpp |
cocorlow | 56:888379912f81 | 193 | |
cocorlow | 56:888379912f81 | 194 | // setup.cpp |
cocorlow | 56:888379912f81 | 195 | extern void setup(); |
cocorlow | 56:888379912f81 | 196 | extern void calibrate(); |
NaotoMorita | 143:53808e4e684c | 197 | |
NaotoMorita | 143:53808e4e684c | 198 | // preflight.cpp |
NaotoMorita | 143:53808e4e684c | 199 | extern void preflightCalibration(); |
NaotoMorita | 143:53808e4e684c | 200 | extern void preflightCheck(); |
NaotoMorita | 143:53808e4e684c | 201 | extern void setEskfCov(); |
NaotoMorita | 143:53808e4e684c | 202 | |
cocorlow | 56:888379912f81 | 203 | // run.cpp |
cocorlow | 56:888379912f81 | 204 | extern void run(); |
cocorlow | 56:888379912f81 | 205 | |
cocorlow | 56:888379912f81 | 206 | // imu.cpp |
osaka | 88:be349faa1976 | 207 | extern void getIMUval(); |
cocorlow | 56:888379912f81 | 208 | |
osaka | 96:3645f8f9bdd3 | 209 | //gps.cpp |
osaka | 96:3645f8f9bdd3 | 210 | extern void getGPSval(); |
osaka | 97:2b3242c2dd85 | 211 | extern void change_refpos(); |
osaka | 96:3645f8f9bdd3 | 212 | |
NaotoMorita | 73:84ffa0166e6c | 213 | // hil.cpp |
NaotoMorita | 92:00460f6df439 | 214 | extern void getHilIMUval(); |
NaotoMorita | 92:00460f6df439 | 215 | extern void getHilGPSval(); |
NaotoMorita | 73:84ffa0166e6c | 216 | extern float randn(); |
NaotoMorita | 73:84ffa0166e6c | 217 | |
osaka | 109:eb255fc4462b | 218 | //autopilot.cpp |
osaka | 109:eb255fc4462b | 219 | extern void level_flight(); |
osaka | 109:eb255fc4462b | 220 | extern void point_guide(); |
osaka | 109:eb255fc4462b | 221 | extern void turning(); |
osaka | 137:240588882ae2 | 222 | extern void climb(); |
cocorlow | 139:b378528c05f2 | 223 | extern Vector3f calc_vdot(); |
osaka | 109:eb255fc4462b | 224 | |
cocorlow | 56:888379912f81 | 225 | // servo.cpp |
cocorlow | 56:888379912f81 | 226 | extern void calcServoOut(); |
cocorlow | 56:888379912f81 | 227 | |
osaka | 87:89bbbcdb667b | 228 | // transferData.cpp |
osaka | 87:89bbbcdb667b | 229 | extern void sendData2PC(); |
osaka | 87:89bbbcdb667b | 230 | extern void sendTelemetry(); |
osaka | 87:89bbbcdb667b | 231 | extern void writeSDcard(); |
cocorlow | 56:888379912f81 | 232 | |
cocorlow | 56:888379912f81 | 233 | // global.cpp |
cocorlow | 56:888379912f81 | 234 | extern float mapfloat(float x, float in_min, float in_max, float out_min, float out_max); |
cocorlow | 139:b378528c05f2 | 235 | extern void setDiag(Matrix3f& mat, float val); |
cocorlow | 141:725321fe2949 | 236 | extern void setDiag(MatrixXf& mat, float val); |
cocorlow | 141:725321fe2949 | 237 | //extern void setBlockDiag(Matrix3f& mat, float val,int startIndex, int endIndex); |
cocorlow | 56:888379912f81 | 238 | #endif |