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.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-11-10
- Revision:
- 104:20b8caa29185
- Parent:
- 102:1c77ff6e2a85
- Child:
- 106:2d854e92cebb
- Child:
- 107:46e039e12182
File content as of revision 104:20b8caa29185:
#include "global.hpp" // var // communication UsaPack pc(USBTX, USBRX, 115200); // log - tail Serial sd(PG_14,PG_9); Serial twelite(PD_1,PD_0); // 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); MagSphereCalibration 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; float rc[16]; int loop_count = 0; float att_dt = 0.01f; // position Matrix SensorAlignment(3,3); Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll y:pitch z:yaw Vector3 acc; Vector3 accref(0.0f, 0.0f, 9.8f); Vector3 mag; Vector3 magref(0.0f, 0.0f, 0.0f); Vector3 gyro; Vector3 vi; Vector3 pi; float palt; float palt0; int itow_velned; int itow_posllh; bool gpsUpdateFlag = 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}; Vector3 rpy_align( 0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f); // UsaPack valuePack vp; sendPack sp; // HIL bool hilFlag = false; 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(Matrix& mat, float val){ for (int i = 1; i < mat.getCols()+1; i++){ mat(i,i) = val; } } void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){ for (int i = startIndex; i < endIndex+1; i++){ mat(i,i) = val; } }