solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: global.hpp
- Revision:
- 56:888379912f81
- Child:
- 61:c05353850017
diff -r b11c9fef094a -r 888379912f81 global.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/global.hpp Mon May 31 18:59:36 2021 +0000 @@ -0,0 +1,146 @@ +#ifndef __GLOBAL_HPP__ +#define __GLOBAL_HPP__ + +#include "mbed.h" +#include "PIDcontroller.h" +#include "SBUS.hpp" +#include "LoopTicker.hpp" +#include "MPU6050.h" +#include "MAG3110.h" +#include "I2Cdev.h" +#include "FastPWM.h" +#include <cmath> +#include "UsaPack.hpp" +#include "Vector3.hpp" +#include "HAPS_EKF.hpp" + +#define MPU6050_PWR_MGMT_1 0x6B +#define MPU_ADDRESS 0x68 +#define M_PI 3.141592f +#define ACCEL_FSR MPU6050_ACCEL_FS_8 +#define ACCEL_SSF 4096.0f +#define GYRO_FSR MPU6050_GYRO_FS_250 +#define GYRO_SSF 131.0f +#define MPU6050_LPF MPU6050_DLPF_BW_256 +#define gyro_th 20.0f +#define PID_dt 0.015f +#define servoPwmMax 1800.0f +#define servoPwmMin 1200.0f +#define motorPwmMax 2000.0f +#define motorPwmMin 1100.0f +#define N_EEPROM 10 + +// struct union +union U +{ + int i[N_EEPROM]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias + char c[N_EEPROM*4]; // floatはcharの4倍 +}; + +struct valuePack +{ + float dt; + int count; + float acc[3]; + float gyro[3]; + float mag[3]; + float rpy[3]; + float rpy_g[3]; +}; + +// var + +// communication +extern Serial pc; +extern I2C i2c; // sda, scl +extern UsaPack tail; // log - tail +extern SBUS sbus; + +// sensor +extern MPU6050 accelgyro; +extern MAG3110 mag_sensor; + +// io +extern DigitalIn userButton; +extern DigitalIn locdef1; +extern DigitalIn locdef2; + +// control +extern FastPWM servo; +extern PID pitchPID; // rad +extern PID pitchratePID;// rad/s +extern HAPS_EKF ekf; // EKF class + +extern int loop_count; +extern int obs_count; +extern float att_dt; +extern float rc[16]; + +extern int16_t ax, ay, az; +extern int16_t gx, gy, gz; +extern MotionSensorDataUnits mdata; +extern float magval[3]; + +// position +extern Vector3 rpy; // x:roll y:pitch z:yaw +extern Vector3 rpy_g; // x:roll y:pitch z:yaw +extern Vector3 acc; +extern Vector3 accref; +extern Vector3 mag; +extern Vector3 magref; +extern Vector3 dynacc; +extern Vector3 gyro; + +extern Vector3 LPacc; +extern Vector3 LPmag; + +extern int out1, out2; +extern float scaledServoOut[1]; +extern float servoOut[1]; + + +extern float val_thmg; +extern float th_mg; +extern float accnormerr; + +extern int calibrationFlag; +extern int pos_tail; // 0:left 1:center 2:right +extern int agoffset[6]; +extern float magbias[4]; + +extern Vector3 rpy_align; + +// eepromのread writeのためのunionを定義 +extern const int eeprom_address; // EEPROMの3つの入力が全て+より +extern const int eeprom_pointeraddress; + +//// UsaPack +extern const int tail_address[3]; +extern valuePack posValues[3]; + +// function + +// main.cpp + +// setup.cpp +extern void setup(); +extern void calibrate(); + +// run.cpp +extern void run(); + +// imu.cpp +extern void getIMUval(); + +// servo.cpp +extern void calcServoOut(); + +// sd_eeprom.cpp +extern void writeSdcard(); +extern void writeEEPROM(int address, unsigned int eeaddress, char *data, int size); +extern void readEEPROM(int address, unsigned int eeaddress, char *data, int size); + +// global.cpp +extern float mapfloat(float x, float in_min, float in_max, float out_min, float out_max); + +#endif \ No newline at end of file