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:
- 93:b827f78a717a
- Parent:
- 92:00460f6df439
- Child:
- 94:579e875a4244
- Child:
- 95:43717535c354
--- a/global.cpp Thu Oct 28 09:44:47 2021 +0000 +++ b/global.cpp Fri Oct 29 13:30:24 2021 +0000 @@ -31,6 +31,7 @@ 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, 0.98f); @@ -54,7 +55,7 @@ float motorOut[1] = {1100.0f}; int calibrationFlag = 0; -int agoffset[6] = {0, 0, 0, -584, -94, 37}; +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}; @@ -70,7 +71,7 @@ sendPack sp; // HIL -bool hilFlag = true; +bool hilFlag = false; bool serialControlSource = false; bool serialParamSource = false; int checkParamSerial[5] = {0,0,0,0,0}; @@ -79,3 +80,15 @@ { 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; + } +}