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-07-20
- Revision:
- 75:a505b9896da1
- Parent:
- 74:f67062e7813e
- Child:
- 76:7fd3ac1afe3e
File content as of revision 75:a505b9896da1:
#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); SBUS sbus(PD_5, PD_6); Joystick joystick(PC_3,PF_3,PF_5); // control 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.5f, 0.0, 0.0, PID_dt);//rad/s errStateEKF ekf; // EKF class float rc[16]; 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 dynacc; Vector3 mag; Vector3 magref(0.65f, 0.0f, 0.75f); Vector3 gyro; 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; int agoffset[6] = {0, 0, 0, -578, -88, 31}; 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(8.201975f*M_PI/180.0f, -0.250809f*M_PI/180.0f, 0.0f*M_PI/180.0f ); // UsaPack valuePack vp; // HIL bool hilFlag = false; 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; }