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
global.cpp
- Committer:
- NaotoMorita
- Date:
- 22 months ago
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
File content as of revision 143:53808e4e684c:
#include "global.hpp" // var // communication UsaPack pc(USBTX, USBRX, 115200); // log - tail UsaPack sd(PG_14,PG_9, 115200); UsaPack twelite(PD_1,PD_0,38400); // io DigitalIn userButton(USER_BUTTON); SBUS sbus(PD_5, PD_6); I2C i2c(PB_9, PB_8); LSM9DS1 lsm(i2c); LPS lps(i2c); GPSUBX_UART gps(PF_7, PE_7); CalibrateMagneto magCalibrator; float magres = 0.0f; // control Timer _t; 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.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 Autopilot autopilot; float roll_obj; float pitch_obj; float dT_obj; float rc[16]; int loop_count = 0; float att_dt = 0.01f; // position Matrix3f SensorAlignmentAG; Matrix3f SensorAlignmentMAG; Vector3f euler; Vector3f acc; Vector3f mag; Vector3f magref(0.0f, 0.0f, 0.0f); Vector3f gyro; Vector3f vi; Vector3f pi; float palt; float palt0; float dynaccnorm2; int itow_status = 0; 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; 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; float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; // UsaPack valuePack vp; sendPack sp; logPack lp; telemetryPack tp; // HIL bool hilFlag = true; int16_t hilDataOut = 0; 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; } 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; } } //void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex){ // for (int i = startIndex; i < endIndex+1; i++){ // mat(i,i) = val; // } //}