solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: imu.cpp
- Revision:
- 92:00460f6df439
- Parent:
- 89:c9f64bd655d9
- Child:
- 93:b827f78a717a
--- a/imu.cpp Tue Oct 26 05:37:25 2021 +0000 +++ b/imu.cpp Thu Oct 28 09:44:47 2021 +0000 @@ -5,11 +5,25 @@ lsm.readAccel(); lsm.readMag(); lsm.readGyro(); + + acc.x = lsm.ax * 9.8f - agoffset[0]; + acc.y = lsm.ay * 9.8f - agoffset[1]; + acc.z = lsm.az * 9.8f - agoffset[2]; + gyro.x = (lsm.gx * M_PI / 180.0f) - agoffset[3]; + gyro.y = (lsm.gy * M_PI / 180.0f) - agoffset[4]; + gyro.z = (lsm.gz * M_PI / 180.0f) - agoffset[5]; + mag.x = lsm.mx; + mag.y = lsm.my; + mag.z = lsm.mz; + + float pressure = lps.readPressureMillibars(); + palt = lps.pressureToAltitudeMeters(pressure); + //printf("%f %f %f %f %f %f %f %f %f\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz); //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz); //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz); //float pressure = lps.readPressureMillibars(); //float altitude = lps.pressureToAltitudeMeters(pressure); //float temperature = lps.readTemperatureC(); - //printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature); + //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature); } \ No newline at end of file