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
Diff: global.cpp
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
diff -r e1b8357d1ea4 -r 53808e4e684c global.cpp --- a/global.cpp Fri Dec 10 11:20:13 2021 +0000 +++ b/global.cpp Fri Jun 24 05:44:34 2022 +0000 @@ -25,8 +25,9 @@ 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 +PID yawratePID(2.0f, 0.0, 0.0, PID_dt);//rad/s +PID climbratePID(1.0f, 0.0, 0.0, PID_dt);//rad/s solaESKF eskf; // ESKF class -int obsCount = 0; Autopilot autopilot; float roll_obj; float pitch_obj; @@ -39,9 +40,7 @@ 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; @@ -54,13 +53,11 @@ bool gpsUpdateFlag = false; bool gpsLlh0Fixed = false; bool headingUpdateFlag = false; +bool hinf_flag = 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}; @@ -70,14 +67,6 @@ 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; @@ -87,9 +76,7 @@ // HIL bool hilFlag = true; -bool serialControlSource = false; -bool serialParamSource = false; -int checkParamSerial[5] = {0,0,0,0,0}; +int16_t hilDataOut = 0; float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) { @@ -98,11 +85,21 @@ 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;