solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: preflight.cpp
- Revision:
- 144:b3a713b4f1c4
- Parent:
- 143:53808e4e684c
--- a/preflight.cpp Fri Jun 24 05:44:34 2022 +0000 +++ b/preflight.cpp Wed Jun 29 07:57:10 2022 +0000 @@ -42,20 +42,25 @@ } // sbusデータの読み込み for (int i =0 ; i < 16;i ++){ - rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input - } - if (rc[4]>-0.3f && rc[6] < -0.3f){ - preflightCheck = false; - twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n"); + rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input } - if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){ + if (rc[7] > 0.3f){ preflightCheck = false; - twelite.serial.printf("PreFlight Check : no gps lock\r\n"); + motor1.pulsewidth_us(motorPwmMin); + motor2.pulsewidth_us(motorPwmMin); + motor3.pulsewidth_us(motorPwmMin); + motor4.pulsewidth_us(motorPwmMin); + motor5.pulsewidth_us(motorPwmMin); + motor6.pulsewidth_us(motorPwmMin); + twelite.serial.printf("PreFlight Check : switch arming position\r\n"); + pc.serial.printf("PreFlight Check : switch arming position\r\n"); } - preflightCheck = true; + + if(preflightCheck == true){ break; } + pc.serial.printf("PreFlight Check : failed\r\n"); } } twelite.serial.printf("PreFlight Check Completed\r\n"); @@ -68,12 +73,12 @@ eskf.setPhatPosition(2.0f,2.0f); eskf.setPhatVelocity(1.01f,1.01f); eskf.setPhatAngleError(0.1f); - eskf.setPhatAccBias(0.5f); + eskf.setPhatAccBias(0.0001f); eskf.setPhatGyroBias(0.0001f); eskf.setPhatGravity(0.0000001f); eskf.setQVelocity(0.1f,0.2f); eskf.setQAngleError(0.0000025f); - eskf.setQAccBias(0.000001f); + eskf.setQAccBias(0.0000001f); eskf.setQGyroBias(0.0000001f); } \ No newline at end of file