Diff: global.cpp

Revision:

63:9c4973a98600

Parent:

62:ef10fd919f7b

Child:

67:41fcdfb7cc5a

--- a/global.cpp	Wed Jun 09 07:50:18 2021 +0000
+++ b/global.cpp	Tue Jun 22 02:02:08 2021 +0000
@@ -3,10 +3,8 @@
 // var
 
 // communication
-Serial pc(USBTX, USBRX);
 I2C i2c(PB_9,PB_8);  // sda, scl
-UsaPack tail(PG_14, PG_9, 57600); // log - tail
-SBUS sbus(PD_5, PD_6);
+UsaPack tail(USBTX, USBRX, 57600); // log - tail
 
 // sensor
 MPU6050 accelgyro;
@@ -15,19 +13,16 @@
 
 // io
 DigitalIn userButton(USER_BUTTON);
-DigitalIn locdef1(PG_2);
-DigitalIn locdef2(PG_3);
-
+CalibrateMagneto joyCalibrator;
 // control
-FastPWM servo(PE_11);
+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
 HAPS_EKF ekf; // EKF class
 
 int loop_count = 0;
-int obs_count = 0;
 float att_dt = 0.01f;
-float rc[16];
 
 int16_t ax, ay, az;
 int16_t gx, gy, gz;
@@ -36,42 +31,25 @@
 
 // position
 Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
-Vector3 rpy_g(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 dynacc;
 Vector3 gyro;
 
-Vector3 LPacc(0.0f, 0.0f, 0.0f);
-Vector3 LPmag(0.0f, 0.0f, 0.0f);
-
-int out1, out2;
-float scaledServoOut[1] = {0.0f};
-float servoOut[1] = {1500.0f};
-
-float val_thmg = 0.0f;
-float sigma_thmg = 0.0f;
-float th_mg = 0.0f;
-float accnormerr = 0.0f;
-float sigma_accnorm = 0.0f;
+float scaledServoOut[2] = {0.0f,0.0f};
+float servoOut[2] = {1500.0f,1500.0f};
 
 int calibrationFlag = 0;
-int pos_tail = 0; // 0:left 1:center 2:right
 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);
+Vector3 rpy_align(0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f);
 
-// eepromのread writeのためのunionを定義
-const int eeprom_address = 0xAE; // EEPROMの3つの入力が全て+より
-const int eeprom_pointeraddress = 0;
 
 // UsaPack
-const int tail_address[3] = {1111, 2222, 3333};
-valuePack posValues[3];
+valuePack posValues;
 
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
 {