航空研究会
a
Fork of
Estrela_v12
by 
Bot Furukawa
Diff: main.cpp
- Revision:
- 1:7985dc23ede7
- Parent:
- 0:248f3186c666
--- a/main.cpp Sat May 20 10:05:35 2017 +0000
+++ b/main.cpp Fri Sep 07 08:28:42 2018 +0000
@@ -1,9 +1,14 @@
+//skipper動作確認用プログラム
+//加速度，ジャイロ，地磁気，気圧，の４つのセンサを確認可能
+//シリアル受信側にはuarttestの受信のプログラムを使用する
+
#include "mbed.h"
//#include "rtos.h"
#include "MPU9250.h"
#include "BMP280.h"
#include "SkipperSv2.h"
#include "Estrela.h"
+#include "math.h"
//#include "stm32f4xx_hal_iwdg.h"
@@ -15,6 +20,15 @@
#define SBUS_SIGNAL_LOST 0x01
#define SBUS_SIGNAL_FAILSAFE 0x03
+
+
+
+
+
+
+
+
+
//Serial pc(TX, RX);
RawSerial sbus_(PA_9, PA_10); //SBUS
@@ -698,24 +712,22 @@
//wait_ms(50);
}
+/*動作確認用uart通信*/
+Serial g_Serial1(USBTX,USBRX);
+Serial g_serial2(PA_2, PA_3);
+/*気圧センサ*/
+BMP280 bmp(PC_9, PA_8);
+
+
int main()
{
- //初期化開始
- led1 = 1;
- led2 = 1;
- led3 = 0;
- led4 = 0;
+
//SBus用バッファ初期化
- int count_initsbus;
- for (count_initsbus = 0; count_initsbus < 25; count_initsbus++) {buf_sbus[count_initsbus] = 0;}
-
//pc.baud(115200); //パソコン通信用シリアルのボードレート
//frq.start();
- init_Servo();
- init_sbus();
- disp_clock();
+
//---9軸センサー(MPU9250)初期化---
@@ -728,37 +740,7 @@
uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
- if (whoami == 0x71){ // WHO_AM_I should always be 0x68
- pc.printf("MPU9250 is online...\n\r");
- //wait(1);
-
- mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
- mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
- //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
- //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
- //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
- //pc.printf("x accel bias = %f\n\r", accelBias[0]);
- //pc.printf("y accel bias = %f\n\r", accelBias[1]);
- //pc.printf("z accel bias = %f\n\r", accelBias[2]);
- wait(1);
-
- mpu9250.initMPU9250();
- pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
- mpu9250.initAK8963(magCalibration);
- pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
- pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
- pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
- if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
- if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
- if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
- if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
- wait(1);
-
- }else{
- pc.printf("Could not connect to MPU9250: \n\r");
- pc.printf("%#x \n", whoami);
- while(1) ; // Loop forever if communication doesn't happen
- }
+
mpu9250.getAres(); // Get accelerometer sensitivity
mpu9250.getGres(); // Get gyro sensitivity
@@ -775,177 +757,48 @@
//---9軸センサー(MPU9250)初期化終わり---
- led1 = 0;
- led2 = 0;
- led3 = 0;
- led4 = 0;
+
+
+
- pc.printf("All initialized\r\n");
+ /*ここからskippre test*/
+ g_serial2.baud(9600);
+
+ //BMP280を初期化 引数にenum型のFilterselectionを入れると
+ //それに合わせた初期化をしてくれる Readme参照
+ //引数いれないとフィルタなしで初期化する
+ bmp.initialize(BMP280::INDOOR_NAVIGATION);
- while (true) { //無限ループ
- //for(i=0;i<8;i++){
- //pc.printf("%x ", buf_sbus[i]);
- //pc.printf("ch%d=%d ", i+1, channels[i]);
- //}
- //pc.printf("\n");
- sensing();
- wait_ms(50);
+ while(1){
+ //気圧を測定 気圧測定時には温度補正のために先に温度を測る必要があるが，
+ //getPressureは勝手に測るのでこれだけでよい
+ //単位はhPa
+ float press = bmp.getPressure();
-
- switch(OperationMode){ //変数OperationModeの値で場合分け
- case GROUNDCHECK: //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行)
- if(!CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7,8が両方offなら
- OperationMode=AUTOLOOP; //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入
- pc.printf("go to autoloop\r\n");
- led1 = 0;
- led2 = 0;
- }else{
- led1 = 1;
- led2 = 1;
- }
- //変数update_modeはSBus(=プロポ＝操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数
- update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す)
- // pc.printf("groundcheck mode\r\n");
-
- break;
- case AUTOLOOP: //水平旋回モード
- //t.reset();
- if(CheckSW_up(7)){ //チャンネル7がonなら
- //t.reset();
- Auto_Loop(); //関数Auto_Loop実行
- led1 = 1;
- led2 = 0;
- //Auto_Loop();
- //pc.printf("Auto Loop Now\r\n");
- //autoloop.set_priority(osPriorityAboveNormal);
- /*
- while(true){
- if(!CheckSW_up(7)){
- autoloop.terminate();
- break;
- }
- }
- */
- }else{
- update_mode = UPD_MANUAL;
- //pc.printf("manual Now\r\n");
- led1 = 0;
- led2 = 0;
-
- /*debug*/
- //k++;
- //wait(50);
- //if(k>100) pwm[6]=2000;
- }
-
- if(!CheckSW_up(7)&&CheckSW_up(8)){ //チャンネル7がoff、8がonなら
- OperationMode=AUTOMOBIUS;
- pc.printf("go to auto8\r\n");
- }
- break;
- case AUTOMOBIUS: //8の字旋回モード
- if(CheckSW_up(7)){
- Auto_Mobius();
- led1 = 1;
- led2 = 1;
- //pc.printf("Auto Mobius Now\r\n");
- }else{
- update_mode = UPD_MANUAL;
- //pc.printf("manual Now\r\n");
- led1 = 0;
- led2 = 1;
- }
-
- if(!CheckSW_up(7)&&!CheckSW_up(8)){
- OperationMode=AUTOCLIMB;
- pc.printf("go to autoclimb\r\n");
- }
- break;
- case AUTOCLIMB: //上昇旋回モード
- if(CheckSW_up(7)){
- Auto_Climb();
- led1 = 1;
- led2 = 0;
- //pc.printf("Auto Climb Now\r\n");
- }else{
- update_mode = UPD_MANUAL;
- //pc.printf("manual Now\r\n");
- led1 = 0;
- led2 = 0;
- }
-
- if(!CheckSW_up(7)&&CheckSW_up(8)){
- OperationMode=AUTOGLIDE;
- pc.printf("go to manual\r\n");
- }
- break;
-
- case AUTOGLIDE: //自動滑空モード
- if(CheckSW_up(7)){
- Auto_Glide();
- led1 = 1;
- led2 = 1;
- //pc.printf("Auto Glide Now\r\n");
- }else{
- update_mode = UPD_MANUAL;
- //pc.printf("manual Now\r\n");
- led1 = 0;
- led2 = 1;
- }
-
- if(!CheckSW_up(7)&&!CheckSW_up(8)){
- OperationMode=AUTOLANDING;
- pc.printf("go to autoloop\r\n");
- }
- break;
-
- case AUTOLANDING:
- led3 = 1;
- led4 = 1; //赤外線
- if(CheckSW_up(7)){
- Auto_Landing();
- led1 = 1;
- led2 = 0;
- //pc.printf("Auto Landing Now\r\n");
- }else{
- update_mode = UPD_MANUAL;
- //pc.printf("manual Now\r\n");
- led1 = 0;
- led2 = 0;
- }
-
- if(!CheckSW_up(7)&&CheckSW_up(8)){
- OperationMode=AUTOLOOP;
- //pc.printf("go to manualmade\r\n");
- }
- break;
-
- case MANUALMODE:
- update_mode = UPD_MANUAL;
- led1 = 0;
- led2 = 1;
- led4 = 1;
-
- if(!CheckSW_up(7)&&!CheckSW_up(8)){
- OperationMode=AUTOLANDING;
- pc.printf("go to autolanding\r\n");
- }
- break;
-
- }
-
-
-
-
- /*
- if(CheckSW_up(7)){
- pc.printf("true\r\n");
- Auto_loop();
-
- }else{
- pc.printf("false\r\n");
- update_manual();
- }*/
+ //もし温度自分で取得する場合，getPressureの引数をtrueにすれば処理を早くできるが，
+ //その場合getPressureの直前に温度を測る必要がある
+ //float temp = bmp.getTemperature();
+ //float press = bmp.getPressure(true);
+
+
+ //気圧と高度の近似換算式 Readme参照
+ //float height = (float)(44.331514 - 11.880516 * pow(press, 0.1902632)) * 1000);
+
+
+
+
+
+ sensing();
+
+ g_serial2.printf("ax,%f ay,%f az,%f\r\n",ax,ay,az);
+ g_serial2.printf("gx,%f gy,%f gz,%f\r\n",gx,gy,gz);
+ g_serial2.printf("mx,%f my,%f mz,%f\r\n",mx,my,mz);
+ g_serial2.printf("press,%f\r\n\r\n",press);
+
+ wait(1.0);
+
}
+
+
}