航空研究会
/
Skipper_operation
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Estrela_v12
by 
Bot Furukawa
Revision 1:7985dc23ede7, committed 2018-09-07
- Comitter:
- TUATBM
- Date:
- Fri Sep 07 08:28:42 2018 +0000
- Parent:
- 0:248f3186c666
- Commit message:
- a
Changed in this revision
| BMP280.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/BMP280.lib Sat May 20 10:05:35 2017 +0000 +++ b/BMP280.lib Fri Sep 07 08:28:42 2018 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/12104404/code/BMP280/#c72b726c7dc9 +https://os.mbed.com/teams/1519026547/code/BMP280/#2dc7ede4ac55
--- 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);
+
}
+
+
}