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
- Committer:
- NaotoMorita
- Date:
- 2022-06-29
- Revision:
- 144:b3a713b4f1c4
- Parent:
- 143:53808e4e684c
File content as of revision 144:b3a713b4f1c4:
#include "global.hpp" // var // communication UsaPack pc(USBTX, USBRX, 115200); // log - tail UsaPack sd(PG_14,PG_9, 115200); UsaPack twelite(PD_1,PD_0,38400); // io DigitalIn userButton(USER_BUTTON); SBUS sbus(PD_5, PD_6); I2C i2c(PB_9, PB_8); LSM9DS1 lsm(i2c); LPS lps(i2c); GPSUBX_UART gps(PF_7, PE_7); CalibrateMagneto magCalibrator; float magres = 0.0f; // control Timer _t; FastPWM motor1(PE_9); FastPWM motor2(PE_11); FastPWM motor3(PE_13); FastPWM motor4(PA_6); FastPWM motor5(PA_0); FastPWM motor6(PB_10); PID pitchPID(0.05f,0.0f,0.0f,PID_dt); //rad PID pitchratePID(0.05f, 0.0f, 0.0f, PID_dt);//rad/s PID rollPID(0.05f,0.0f,0.0f,PID_dt); PID rollratePID(0.05f, 0.0, 0.0, PID_dt);//rad/s PID yawratePID(0.3f, 0.0, 0.0, PID_dt);//rad/s PID vxPID(0.5f, 0.1f, 0.0f, PID_dt); PID vyPID(0.5f, 0.1f, 0.0f, PID_dt); PID vzPID(0.05f, 0.0f, 0.0f, PID_dt); solaESKF eskf; // ESKF class float rc[16]; int loop_count = 0; float att_dt = 0.01f; // position Matrix3f SensorAlignmentAG = Matrix3f::Zero(); Matrix3f SensorAlignmentMAG = Matrix3f::Zero(); Vector3f euler = Vector3f::Zero(); Vector3f acc = Vector3f::Zero(); Vector3f mag = Vector3f::Zero(); Vector3f magref = Vector3f::Zero(); Vector3f gyro = Vector3f::Zero(); Vector3f vi = Vector3f::Zero(); Vector3f pi = Vector3f::Zero(); float palt; float palt0; float dynaccnorm2; int itow_status = 0; bool gpsUpdateFlag = false; bool gpsLlh0Fixed = false; bool headingUpdateFlag = false; bool hinf_flag = false; float de = 0.0f; float da = 0.0f; float dr = 0.0f; float dT = 0.0f; float scaledMotorOut[6]= {-1.0f,-1.0f,-1.0f,-1.0f,-1.0f,-1.0f}; float motorOut[6] = {1100.0f,1100.0f,1100.0f,1100.0f,1100.0f,1100.0f}; float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; // UsaPack valuePack vp; sendPack sp; logPack lp; telemetryPack tp; // HIL bool hilFlag = false; int16_t hilDataOut = 0; float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } void setDiag(Matrix3f& mat, float val){ for (int i = 0; i < mat.cols(); i++){ for (int j = 0; j < mat.rows(); j++){ mat(i,j) = 0.0f; } } for (int i = 0; i < mat.cols(); i++){ mat(i,i) = val; } } void setDiag(MatrixXf& mat, float val){ for (int i = 0; i < mat.cols(); i++){ for (int j = 0; j < mat.rows(); j++){ mat(i,j) = 0.0f; } } for (int i = 0; i < mat.cols(); i++) { mat(i, i) = val; } } //void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex){ // for (int i = startIndex; i < endIndex+1; i++){ // mat(i,i) = val; // } //}