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:
- cocorlow
- Date:
- 2021-12-10
- Revision:
- 141:725321fe2949
- Parent:
- 140:53dbdb207542
- Child:
- 143:53808e4e684c
File content as of revision 141:725321fe2949:
#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 servoRight(PE_9); FastPWM servoLeft(PE_11); FastPWM servoThrust(PE_13); PID pitchPID(10.0f,0.0f,0.0f,PID_dt); //rad PID pitchratePID(1.0f, 0.0f, 0.0f, PID_dt);//rad/s PID rollPID(5.0f,0.0f,0.0f,PID_dt); PID rollratePID(0.05f, 0.0, 0.0, PID_dt);//rad/s solaESKF eskf; // ESKF class int obsCount = 0; Autopilot autopilot; float roll_obj; float pitch_obj; float dT_obj; float rc[16]; int loop_count = 0; float att_dt = 0.01f; // position Matrix3f SensorAlignmentAG; Matrix3f SensorAlignmentMAG; Vector3f euler; Vector3f rpy(0.0f, 0.0f, 0.0f); // x:roll y:pitch z:yaw Vector3f acc; Vector3f accref(0.0f, 0.0f, 9.8f); Vector3f mag; Vector3f magref(0.0f, 0.0f, 0.0f); Vector3f gyro; Vector3f vi; Vector3f pi; float palt; float palt0; float dynaccnorm2; int itow_status = 0; bool gpsUpdateFlag = false; bool gpsLlh0Fixed = false; bool headingUpdateFlag = false; float de = 0.0f; float da = 0.0f; float dT = 0.0f; MedianFilter accMedian(3); MedianFilter gyroMedian(3); MedianFilter magMedian(3); float scaledServoOut[2]= {0.0f,0.0f}; float scaledMotorOut[1]= {-1.0f}; float servoOut[2] = {1500.0f,1500.0f}; float motorOut[1] = {1100.0f}; int calibrationFlag = 0; float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; float magbias[4] = {-134.817047, 127.833481, -152.631836, 44.508556}; float magbiasMin[3] = {-174.831711, 93.465691, -188.617172}; float magbiasMax[3] = {-110.413963, 162.321548, -122.566071}; float accMin[3] = {-0.963692, -0.974141, -1.012899}; float accMax[3] = { 1.025954, 0.974748, 0.987155}; Vector3f rpy_align( 0.0f*M_PI_F/180.0f, 5.0f*M_PI_F/180.0f, 0.0f*M_PI_F/180.0f); // UsaPack valuePack vp; sendPack sp; logPack lp; telemetryPack tp; // HIL bool hilFlag = true; bool serialControlSource = false; bool serialParamSource = false; int checkParamSerial[5] = {0,0,0,0,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++){ mat(i,i) = val; } } void setDiag(MatrixXf& mat, float val){ 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; // } //}