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:
- NaotoMorita
- Date:
- 2021-06-22
- Revision:
- 67:41fcdfb7cc5a
- Parent:
- 63:9c4973a98600
- Child:
- 68:b9f6938fab9d
File content as of revision 67:41fcdfb7cc5a:
#include "global.hpp" // var // communication I2C i2c(PB_9,PB_8); // sda, scl UsaPack pc(USBTX, USBRX, 57600); // log - tail // sensor MPU6050 accelgyro; MAG3110 mag_sensor(PB_9,PB_8); CalibrateMagneto magCalibrator; // io DigitalIn userButton(USER_BUTTON); CalibrateMagneto joyCalibrator; // control FastPWM elevServo(PE_11); FastPWM rudServo(PE_13); PID pitchPID(5.0, 0,0, PID_dt); // rad PID pitchratePID(0.5, 0.0, 0.0, PID_dt); // rad/s errStateEKF ekf; // EKF class int loop_count = 0; float att_dt = 0.01f; int16_t ax, ay, az; int16_t gx, gy, gz; MotionSensorDataUnits mdata; float magval[3] = {1.0f, 0.0f, 0.0f}; // position Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll y:pitch z:yaw Vector3 acc; Vector3 accref(0.0f, 0.0f, -0.98f); Vector3 mag; Vector3 magref(0.65f, 0.0f, 0.75f); Vector3 gyro; float scaledServoOut[2] = {0.0f,0.0f}; float servoOut[2] = {1500.0f,1500.0f}; int calibrationFlag = 0; int agoffset[6] = {0, 0, 0, 386, -450, 48}; float magbiasMin[3] = {0.0f, 0.0f, 0.0f}; float magbiasMax[3] = {0.0f, 0.0f, 0.0f}; Vector3 rpy_align(0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f); // UsaPack valuePack posValues; 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; }