Bot Furukawa
/
Estrela_v12
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.
Diff: main.cpp
- Revision:
- 0:248f3186c666
diff -r 000000000000 -r 248f3186c666 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat May 20 10:05:35 2017 +0000
@@ -0,0 +1,951 @@
+#include "mbed.h"
+//#include "rtos.h"
+#include "MPU9250.h"
+#include "BMP280.h"
+#include "SkipperSv2.h"
+#include "Estrela.h"
+//#include "stm32f4xx_hal_iwdg.h"
+
+
+#define UPD_MANUAL 0
+#define UPD_AUTO 1
+#define UPD_LANDING 2
+
+#define SBUS_SIGNAL_OK 0x00
+#define SBUS_SIGNAL_LOST 0x01
+#define SBUS_SIGNAL_FAILSAFE 0x03
+
+//Serial pc(TX, RX);
+RawSerial sbus_(PA_9, PA_10); //SBUS
+
+PwmOut servo1(PC_6); // TIM3_CH1
+PwmOut servo2(PC_7); // TIM3_CH2 //PC_7
+PwmOut servo3(PB_0); // TIM3_CH3
+PwmOut servo4(PB_1); // TIM3_CH4
+PwmOut servo5(PB_6); // TIM4_CH1
+PwmOut servo6(PB_7); // TIM4_CH2
+PwmOut servo7(PB_8); // TIM4_CH3 //PB_8
+PwmOut servo8(PB_9); // TIM4_CH4
+
+RawSerial pc(PA_2, PA_3);
+
+DigitalOut led1(PA_0); //緑
+DigitalOut led2(PA_1); //赤
+DigitalOut led3(PB_4); //赤外線
+DigitalOut led4(PB_5);
+
+volatile uint8_t buf_sbus[25];
+volatile int cnt_sbus = 0;
+static int16_t channels[18];
+uint8_t failsafe_status = SBUS_SIGNAL_FAILSAFE;
+static bool flg_ch_update = false;
+//uint8_t i,j,k;
+uint8_t update_mode = 0;
+static uint16_t pwm[8];
+static uint16_t oldTHL;
+
+//9軸センサー関連
+float sum = 0;
+uint32_t sumCount = 0;
+MPU9250 mpu9250;
+Timer t;
+
+/*
+float g_BefGyro[3] = {0,0,0};
+double g_HpfGyro[3] = {0,0,0};
+double g_HpfGyro1[3] = {0,0,0};
+double g_LpfAccel[3] = {0,0,0};
+*/
+
+static int jj = 0;
+
+//IWDG_HandleTypeDef hiwdg;
+
+void disp_clock(){
+ pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
+ pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
+ pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
+ pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
+ pc.printf("\r\n");
+ }
+
+//サーボ初期化関数
+void init_Servo()
+{
+ uint8_t j = 0;
+
+ servo1.period_ms(14);
+ servo1.pulsewidth_us(1500);
+
+ servo2.period_ms(14);
+ servo2.pulsewidth_us(1500);
+
+ servo3.period_ms(14);
+ servo3.pulsewidth_us(1000);
+
+ servo4.period_ms(14);
+ servo4.pulsewidth_us(1500);
+
+ servo5.period_ms(14);
+ servo5.pulsewidth_us(1500);
+
+ servo6.period_ms(14);
+ servo6.pulsewidth_us(1500);
+
+ servo7.period_ms(14);
+ servo7.pulsewidth_us(1500);
+
+ servo8.period_ms(14);
+ servo8.pulsewidth_us(1500);
+
+ for (j=0; j<8; j++) {pwm[j] = 1500;}
+ pc.printf("servo initialized\r\n");
+}
+
+//---setall_servo---
+//pwmをサーボに出力する関数。
+void setall_servo()
+{
+ uint8_t j = 0;
+
+ //if(failsafe_status == SBUS_SIGNAL_OK){
+ servo1.pulsewidth_us(pwm[0]);
+ servo2.pulsewidth_us(pwm[1]);
+ servo3.pulsewidth_us(pwm[2]);
+ servo4.pulsewidth_us(pwm[3]);
+ servo5.pulsewidth_us(pwm[4]);
+ servo6.pulsewidth_us(pwm[5]);
+ servo7.pulsewidth_us(pwm[6]);
+ servo8.pulsewidth_us(pwm[7]);
+ /* }else{
+ servo1.pulsewidth_us(1000);
+ servo2.pulsewidth_us(1000);
+ servo3.pulsewidth_us(1000);
+ servo4.pulsewidth_us(1000);
+ servo5.pulsewidth_us(1000);
+ servo6.pulsewidth_us(1000);
+ servo7.pulsewidth_us(1000);
+ servo8.pulsewidth_us(1000);
+ }*/
+
+ for(j=0;j<8;j++){
+ pc.printf("ch%d=%d ", j+1, pwm[j]);
+ }
+ //pc.printf("time = %d", frq.read_ms());
+ pc.printf("\r\n");
+
+ //frq.reset();
+
+}
+
+
+//---update_pwm---
+//udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
+//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
+void update_pwm()
+{
+ uint8_t j = 0;
+
+ if(flg_ch_update == true){
+ switch(update_mode){ //マニュアルモード,自動モード,自動着陸もモードを切替
+ case UPD_MANUAL: //マニュアルモード
+ for(j=0;j<8;j++){
+ pwm[j] = (channels[j]>>1) + 1000;
+ }
+ oldTHL = pwm[2];
+ jj=0;
+ //pc.printf("update_manual\r\n");
+ break;
+
+ case UPD_AUTO: //自動モード
+ pwm[0] = (channels[0]>>1) + 1000;
+ pwm[1] = Auto_ELE;
+ pwm[2] = Auto_THR;
+ pwm[3] = Auto_RUD;
+ pwm[4] = (channels[4]>>1) + 1000;
+ pwm[5] = (channels[5]>>1) + 1000;
+ pwm[6] = (channels[6]>>1) + 1000;
+ pwm[7] = (channels[7]>>1) + 1000;
+
+ //pc.printf("update_auto\r\n");
+ break;
+
+ case UPD_LANDING: //自動着陸モード
+ pwm[0] = (channels[0]>>1) + 1000;
+ pwm[1] = Auto_ELE;
+ pwm[2] = (channels[2]>>1) + 1000;
+ pwm[3] = (channels[3]>>1) + 1000;
+ pwm[4] = (channels[4]>>1) + 1000;
+ pwm[5] = (channels[5]>>1) + 1000;
+ pwm[6] = (channels[6]>>1) + 1000;
+ pwm[7] = (channels[7]>>1) + 1000;
+ break;
+
+ default: //デフォはマニュアルモード
+ for(j=0;j<8;j++){
+ pwm[j] = (channels[j]>>1) + 1000;
+ }
+
+ jj=0;
+ //pc.printf("update_manual\r\n");
+ break;
+ }
+ }else{
+ pc.printf("0\r\n");
+ }
+ flg_ch_update = false;
+ ///oldTHL = pwm[2];
+ setall_servo();
+}
+
+/*void update_auto()
+{
+ pwm[0] = (channels[0]>>1) + 1000;
+ pwm[1] = Auto_ELE;
+ pwm[2] = Auto_THR;
+ pwm[3] = Auto_RUD;
+ pwm[4] = (channels[4]>>1) + 1000;
+ pwm[5] = (channels[5]>>1) + 1000;
+ pwm[6] = (channels[6]>>1) + 1000;
+ pwm[7] = (channels[7]>>1) + 1000;
+
+ setall_servo();
+ //pc.printf("update_auto\r\n");
+}*/
+
+//---update_Input---
+//catch_sbusで取得したSbusの生データから各チャンネルのpwmを抽出する。
+void update_Input()
+{
+ uint8_t i = 0;
+
+/* for (i=0; i<25; i++){
+ pc.printf("%x ", buf_sbus[i]);
+ }pc.printf("\n");
+*/
+ // clear channels[]
+ for (i=0; i<18; i++) {channels[i] = 0;}
+
+ // reset counters
+ uint8_t byte_in_sbus = 1;
+ uint8_t bit_in_sbus = 0;
+ uint8_t ch = 0;
+ uint8_t bit_in_channel = 0;
+
+ // process actual sbus data
+ for (i=0; i<176; i++) {
+ if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) {
+ channels[ch] |= (1<<bit_in_channel);
+ }
+ bit_in_sbus++;
+ bit_in_channel++;
+
+ if (bit_in_sbus == 8) {
+ bit_in_sbus =0;
+ byte_in_sbus++;
+ }
+ if (bit_in_channel == 11) {
+ bit_in_channel =0;
+ ch++;
+ }
+ }
+ // DigiChannel 1
+ if (buf_sbus[23] & (1<<0)) {
+ channels[16] = 1;
+ }else{
+ channels[16] = 0;
+ }
+ // DigiChannel 2
+ if (buf_sbus[23] & (1<<1)) {
+ channels[17] = 1;
+ }else{
+ channels[17] = 0;
+ }
+ // Failsafe
+ failsafe_status = SBUS_SIGNAL_OK;
+ if (buf_sbus[23] & (1<<2)) {
+ failsafe_status = SBUS_SIGNAL_LOST;
+ }
+ if (buf_sbus[23] & (1<<3)) {
+ failsafe_status = SBUS_SIGNAL_FAILSAFE;
+ }
+ //channels[i] = channels[i]>>1;
+ for (i=0; i<25; i++) {buf_sbus[i] = 0;}
+ cnt_sbus = 0;
+ flg_ch_update = true;
+
+ update_pwm();
+
+
+}
+
+//---catch_sbus---
+//受信機から送られてくるSBus信号(シリアル)を取得する関数
+void catch_sbus()
+{
+ buf_sbus[cnt_sbus] = sbus_.getc();
+ if(buf_sbus[0] == 0x0f)
+ {
+ cnt_sbus ++;
+ }
+ if(cnt_sbus >=25){
+ if(buf_sbus[24] == 0x00){
+ update_Input(); //update_Input実行
+ }else{
+ cnt_sbus = 0;
+ }
+ }
+return;
+}
+
+//---init_sbus---
+//Sbusを初期化する関数
+void init_sbus(){
+ // Set Baudrate
+ sbus_.baud(100000);
+ // Set Datalenght & Frame
+ sbus_.format(8, Serial::Even, 2);
+ //start sbus irq
+ sbus_.attach(&catch_sbus, RawSerial::RxIrq);
+ }
+
+//---CheckSW_up---
+//プロポのスイッチがonかoffか返す関数
+//ch 1~8
+bool CheckSW_up(int8_t ch)
+{
+ uint16_t checkpwm;
+ checkpwm = (channels[ch-1]>>1) + 1000;
+ //pc.printf("ch%d=%d ", ch, checkpwm);
+ if(SWITCH_CHECK > checkpwm){
+ return true;
+ }else{
+ return false;
+ }
+}
+
+
+
+//---sensing---
+//センサーの値を読み込み、各種フィルタをかける(認知)
+void sensing()
+{
+ // If intPin goes high, all data registers have new data
+ if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
+
+ mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
+ // Now we'll calculate the accleration value into actual g's
+ ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ ay = (float)accelCount[1]*aRes - accelBias[1];
+ az = (float)accelCount[2]*aRes - accelBias[2];
+
+ mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
+ // Calculate the gyro value into actual degrees per second
+ gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ gy = (float)gyroCount[1]*gRes - gyroBias[1];
+ gz = (float)gyroCount[2]*gRes - gyroBias[2];
+
+ mpu9250.readMagData(magCount); // Read the x/y/z adc values
+ // Calculate the magnetometer values in milliGauss
+ // Include factory calibration per data sheet and user environmental corrections
+ mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
+ my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+ mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+ }
+
+ Now = t.read_us();
+ deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ lastUpdate = Now;
+
+ sum += deltat;
+ sumCount++;
+
+// if(lastUpdate - firstUpdate > 10000000.0f) {
+// beta = 0.04; // decrease filter gain after stabilized
+// zeta = 0.015; // increasey bias drift gain after stabilized
+// }
+
+ // Pass gyro rate as rad/s
+ mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+ mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+
+ /* Auto_Loop()で50ms待ってるから要らないと思う
+ // Serial print and/or display at 0.5 s rate independent of data rates
+ delt_t = t.read_ms() - count;
+ if (delt_t > 500) { // update LCD once per half-second independent of read rate
+ */
+
+ //pc.printf("ax = %f", 1000*ax);
+ //pc.printf(" ay = %f", 1000*ay);
+ //pc.printf(" az = %f mg\n\r", 1000*az);
+
+ //pc.printf("gx = %f", gx);
+ //pc.printf(" gy = %f", gy);
+ //pc.printf(" gz = %f deg/s\n\r", gz);
+
+ //pc.printf("mx = %f,", mx);
+ //pc.printf(" my = %f,", my);
+ //pc.printf(" mz = %f mG\n\r", mz);
+
+ tempCount = mpu9250.readTempData(); // Read the adc values
+ temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+ //pc.printf(" temperature = %f C\n\r", temperature);
+
+ //pc.printf("q0 = %f\n\r", q[0]);
+ //pc.printf("q1 = %f\n\r", q[1]);
+ //pc.printf("q2 = %f\n\r", q[2]);
+ //pc.printf("q3 = %f\n\r", q[3]);
+
+
+ // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
+ // In this coordinate system, the positive z-axis is down toward Earth.
+ // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
+ // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
+ // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
+ // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
+ // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
+ // applied in the correct order which for this configuration is yaw, pitch, and then roll.
+ // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+ yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+ roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ pitch = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+ pitch *= 180.0f / PI;
+ yaw *= 180.0f / PI;
+ yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+ roll *= 180.0f / PI;
+
+ //pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100));
+ //pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll);
+ //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+
+ count = t.read_ms();
+ sum = 0;
+ sumCount = 0;
+ //}
+}
+
+
+//---CalibrateCompass---
+//グラウンドチェック時にスイッチ7だけをonにしておくことで実行 リポ抜いたらやり直ししなくてはならない
+//使わない予定
+void CalibrateCompass(void)
+{
+ led1 = 1;
+ sensing();
+ uint8_t ii;
+ float mxMAX=mx, mxMIN=mx;
+ float myMAX=my, myMIN=my;
+ magbias[0] = 0; //初期化
+ magbias[1] = 0;
+ //magbias[2] = 0;
+ ii=0;
+ while(1){ //キャリブレーション実行 led2が点滅時に機体を一回転させる xy方向のみ
+ sensing();
+ if(mxMAX < mx) mxMAX = mx;
+ if(mxMIN > mx) mxMIN = mx;
+ if(myMAX < my) myMAX = my;
+ if(myMIN > my) myMIN = my;
+ led2 = !led2;
+ ++ii;
+ wait(50);
+
+ if(!CheckSW_up(7)) break; //スイッチ7を下げて終了
+ }
+ led2 = 0;
+ if(ii<20){ //チャンネル7をすぐ切った場合 元に戻す
+ magbias[0] = MAGBIASX;
+ magbias[1] = MAGBIASY;
+ }else{ //しっかりと値を入れた場合
+ magbias[0] = (mxMAX+mxMIN)/2;
+ magbias[1] = (myMAX+myMIN)/2;
+ }
+ led1 = 0;
+ t.reset(); //タイマーをリセットして終了
+}
+
+
+//---StabiGyro---
+//水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
+//右旋回
+void StabiGyro()
+{
+ if(0 /*oldTHL < 1200*/){ //機体審査用
+ Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN-gy*RUD_DGN);
+ Auto_ELE = ELE_N + -50 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = 1500;
+ }else{
+ if(jj<=0){ //旋回し始め 15
+ Auto_RUD = RUD_N + -65 + int((-12 -roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5
+ Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx
+ Auto_THR = oldTHL + 30;
+ //pc.printf("first%d\r\n",jj);
+ jj++;
+ }else{ //旋回中
+ Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
+ Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL ;
+ }
+ }
+
+ //pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする
+ if(Auto_RUD > RUD_MAX){
+ Auto_RUD = RUD_MAX;
+ }else if(Auto_RUD < RUD_MIN){
+ Auto_RUD = RUD_MIN;
+ }
+ if(Auto_ELE > ELE_MAX){
+ Auto_ELE = ELE_MAX;
+ }else if(Auto_ELE < ELE_MIN){
+ Auto_ELE = ELE_MIN;
+ }
+
+
+
+ //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
+ //Thread::wait(G_MS);
+}
+
+//---StabiGyro_Mobius---
+//8の字旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
+void StabiGyro_Mobius()
+{
+ if(jj<=15){ //右旋回導入
+ Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - (gy + 130)*0.75); //*0.75); //roll -> gy 3.5
+ Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - (gx + 30)*0.8); //pitch -> -gx
+ Auto_THR = oldTHL;
+ //pc.printf("first%d\r\n",jj);
+ jj++;
+ }else if((15<jj) && (jj<=70)){ //右旋回途中
+ Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
+ Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL;
+ jj++;
+ }else if((70<jj) && (jj<=75)){ //旋回遷移1
+ Auto_RUD = RUD_N + 100 + int((16 -roll)*RUDDER_GN - (gy-100)*0.75);
+ Auto_ELE = ELE_N + 20 + int((0 -pitch)*ELEVATOR_GN - (gx + 0)*0.6);
+ Auto_THR = THR_N - 100;
+ //pc.printf("first%d\r\n",jj);
+ jj++;
+ }else if((75<jj) && (jj<=90)){ //旋回遷移2
+ Auto_RUD = RUD_N + 65 + int((12-roll)*RUDDER_GN - (gy-100)*0.75); // 105 30
+ Auto_ELE = ELE_N + -15 + int((-5 - pitch)*ELEVATOR_GN - (gx+30)*0.6); //-75 -15
+ Auto_THR = oldTHL - 20; //-20
+ jj++;
+ }else{ //左旋回途中
+ Auto_RUD = RUD_N + 65 + int((12-roll)*RUDDER_GN - gy*RUD_DGN);
+ Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL;
+ }
+
+ if(Auto_RUD > RUD_MAX){
+ Auto_RUD = RUD_MAX;
+ }else if(Auto_RUD < RUD_MIN){
+ Auto_RUD = RUD_MIN;
+ }
+ if(Auto_ELE > ELE_MAX){
+ Auto_ELE = ELE_MAX;
+ }else if(Auto_ELE < ELE_MIN){
+ Auto_ELE = ELE_MIN;
+ }
+
+ //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
+ //Thread::wait(G_MS);
+}
+
+//---StabiGyro_Climb---
+//上昇旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
+void StabiGyro_Climb()
+{
+ if(jj<=15){ //右旋回導入
+ Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - (gy+130)*RUD_DGN); //*0.75); //roll -> gy 3.5
+ Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - (gx+30)*ELE_DGN); //pitch -> -gx
+ Auto_THR = oldTHL;
+ //pc.printf("first%d\r\n",jj);
+ jj++;
+ }else if((15<jj) && (jj<=136)){ //右旋回途中
+ Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
+ Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL;
+ jj++;
+ /*}else if((136<jj) && (jj<=146)){ //上昇遷移
+ Auto_RUD = RUD_N + -65 - int((-12-roll)*RUDDER_GN - gy*0);
+ Auto_ELE = ELE_N + -80 + int((-22-pitch)*ELEVATOR_GN - gx*0);
+ Auto_THR = THR_N + 100;
+ //pc.printf("first%d\r\n",jj);
+ jj++;*/
+ }else if((136<jj) && (jj<=200)){ //上昇中
+ Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
+ Auto_ELE = ELE_N + -140 + int((-25 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL + 220;
+ jj++;
+ }else{ //水平旋回
+ Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
+ Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL;
+ }
+
+ if(Auto_RUD > RUD_MAX){
+ Auto_RUD = RUD_MAX;
+ }else if(Auto_RUD < RUD_MIN){
+ Auto_RUD = RUD_MIN;
+ }
+ if(Auto_ELE > ELE_MAX){
+ Auto_ELE = ELE_MAX;
+ }else if(Auto_ELE < ELE_MIN){
+ Auto_ELE = ELE_MIN;
+ }
+
+ //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
+ //Thread::wait(G_MS);
+}
+
+//---StabiGyro_Glide---
+//自動滑空のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
+void StabiGyro_Glide()
+{
+ if(jj<=15){
+ Auto_RUD = RUD_N;
+ Auto_ELE = ELE_N + -5 + int (( - pitch)*ELEVATOR_GN - gx*0);
+ Auto_THR = 1000;
+ //pc.printf("first%d\r\n",jj);
+ jj++;
+ }else{
+ Auto_RUD = RUD_N + -30 + int((-8 - roll)*RUDDER_GN - gy*RUD_DGN);
+ Auto_ELE = ELE_N + -25 + int((-5- pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = 1000; //スロットルoff
+ }
+ if(Auto_RUD > RUD_MAX){
+ Auto_RUD = RUD_MAX;
+ }else if(Auto_RUD < RUD_MIN){
+ Auto_RUD = RUD_MIN;
+ }
+ if(Auto_ELE > ELE_MAX){
+ Auto_ELE = ELE_MAX;
+ }else if(Auto_ELE < ELE_MIN){
+ Auto_ELE = ELE_MIN;
+ }
+
+ //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
+ //Thread::wait(G_MS);
+}
+
+void StabiGyro_Landing()
+{
+ if(pwm[0]<1300){
+ Auto_ELE = ELE_N + -10 + int((-5 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL;
+ }else if((1300<=pwm[0])&&(pwm[0]<1800)){
+ Auto_ELE = ELE_N + -15 + int((-8 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL - 30;
+ }else if(1800<=pwm[0]){
+ Auto_ELE = ELE_N + -25 + int((-12 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = 1000;
+ }
+}
+
+void Auto_Loop()
+{
+ //while(true){
+ //for(k=0;k<100;++k){
+ //sensing(); //センサーの値を読み込む(認知)
+ StabiGyro(); //水平旋回のためのラダー、エレベーターのpwmを計算(判断)
+ update_mode = UPD_AUTO; //オートモード
+ //StabiAccel();
+ //Auto_ELE+=145;
+ //Auto_RUD-=80;
+ //Servos.Auto2Servo();
+ //Thread::wait(50);
+ /*
+ if(!CheckSW_up(7)){
+ pc.printf("go to manual\n");
+ break;
+ }*/
+ //wait_ms(50);
+ //}
+ //k=0; //for debug
+ //pwm[6]=1000;
+}
+
+void Auto_Mobius()
+{
+ //sensing();
+ StabiGyro_Mobius();
+ update_mode = UPD_AUTO;
+ //wait_ms(50);
+}
+
+void Auto_Climb()
+{
+ //sensing();
+ StabiGyro_Climb();
+ update_mode = UPD_AUTO;
+ //wait_ms(50);
+}
+
+void Auto_Glide()
+{
+ //sensing();
+ StabiGyro_Glide();
+ update_mode = UPD_AUTO;
+ //wait_ms(50);
+}
+
+void Auto_Landing()
+{
+ //sensing();
+ StabiGyro_Landing();
+ update_mode = UPD_MANUAL;
+ //wait_ms(50);
+}
+
+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)初期化---
+
+ //Set up I2C
+ i2c.frequency(400000); // use fast (400 kHz) I2C
+ pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+ t.start();
+
+ // Read the WHO_AM_I register, this is a good test of communication
+ 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
+ mpu9250.getMres(); // Get magnetometer sensitivity
+ //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+ //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+ //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+ magbias[0] = MAGBIASX; //+470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
+ magbias[1] = MAGBIASY; //+120.; // User environmental y-axis correction in milliGauss
+ magbias[2] = MAGBIASZ; //+125.; // User environmental z-axis correction in milliGauss
+
+ t.start();
+ pc.printf("mpu9250 initialized\r\n");
+
+ //---9軸センサー(MPU9250)初期化終わり---
+
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;
+
+ pc.printf("All initialized\r\n");
+
+
+ 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);
+
+
+ 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();
+ }*/
+ }
+}