Bot Furukawa / Mbed 2 deprecated 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.

Dependencies:   BMP280 mbed

main.cpp@0:248f3186c666, 2017-05-20 (annotated)

Committer:
TUATBM
Date:
Sat May 20 10:05:35 2017 +0000
Revision:
0:248f3186c666
170520

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TUATBM 0:248f3186c666 1 #include "mbed.h"
TUATBM 0:248f3186c666 2 //#include "rtos.h"
TUATBM 0:248f3186c666 3 #include "MPU9250.h"
TUATBM 0:248f3186c666 4 #include "BMP280.h"
TUATBM 0:248f3186c666 5 #include "SkipperSv2.h"
TUATBM 0:248f3186c666 6 #include "Estrela.h"
TUATBM 0:248f3186c666 7 //#include "stm32f4xx_hal_iwdg.h"
TUATBM 0:248f3186c666 8
TUATBM 0:248f3186c666 9
TUATBM 0:248f3186c666 10 #define UPD_MANUAL 0
TUATBM 0:248f3186c666 11 #define UPD_AUTO 1
TUATBM 0:248f3186c666 12 #define UPD_LANDING 2
TUATBM 0:248f3186c666 13
TUATBM 0:248f3186c666 14 #define SBUS_SIGNAL_OK 0x00
TUATBM 0:248f3186c666 15 #define SBUS_SIGNAL_LOST 0x01
TUATBM 0:248f3186c666 16 #define SBUS_SIGNAL_FAILSAFE 0x03
TUATBM 0:248f3186c666 17
TUATBM 0:248f3186c666 18 //Serial pc(TX, RX);
TUATBM 0:248f3186c666 19 RawSerial sbus_(PA_9, PA_10); //SBUS
TUATBM 0:248f3186c666 20
TUATBM 0:248f3186c666 21 PwmOut servo1(PC_6); // TIM3_CH1
TUATBM 0:248f3186c666 22 PwmOut servo2(PC_7); // TIM3_CH2 //PC_7
TUATBM 0:248f3186c666 23 PwmOut servo3(PB_0); // TIM3_CH3
TUATBM 0:248f3186c666 24 PwmOut servo4(PB_1); // TIM3_CH4
TUATBM 0:248f3186c666 25 PwmOut servo5(PB_6); // TIM4_CH1
TUATBM 0:248f3186c666 26 PwmOut servo6(PB_7); // TIM4_CH2
TUATBM 0:248f3186c666 27 PwmOut servo7(PB_8); // TIM4_CH3 //PB_8
TUATBM 0:248f3186c666 28 PwmOut servo8(PB_9); // TIM4_CH4
TUATBM 0:248f3186c666 29
TUATBM 0:248f3186c666 30 RawSerial pc(PA_2, PA_3);
TUATBM 0:248f3186c666 31
TUATBM 0:248f3186c666 32 DigitalOut led1(PA_0); //緑
TUATBM 0:248f3186c666 33 DigitalOut led2(PA_1); //赤
TUATBM 0:248f3186c666 34 DigitalOut led3(PB_4); //赤外線
TUATBM 0:248f3186c666 35 DigitalOut led4(PB_5);
TUATBM 0:248f3186c666 36
TUATBM 0:248f3186c666 37 volatile uint8_t buf_sbus[25];
TUATBM 0:248f3186c666 38 volatile int cnt_sbus = 0;
TUATBM 0:248f3186c666 39 static int16_t channels[18];
TUATBM 0:248f3186c666 40 uint8_t failsafe_status = SBUS_SIGNAL_FAILSAFE;
TUATBM 0:248f3186c666 41 static bool flg_ch_update = false;
TUATBM 0:248f3186c666 42 //uint8_t i,j,k;
TUATBM 0:248f3186c666 43 uint8_t update_mode = 0;
TUATBM 0:248f3186c666 44 static uint16_t pwm[8];
TUATBM 0:248f3186c666 45 static uint16_t oldTHL;
TUATBM 0:248f3186c666 46
TUATBM 0:248f3186c666 47 //9軸センサー関連
TUATBM 0:248f3186c666 48 float sum = 0;
TUATBM 0:248f3186c666 49 uint32_t sumCount = 0;
TUATBM 0:248f3186c666 50 MPU9250 mpu9250;
TUATBM 0:248f3186c666 51 Timer t;
TUATBM 0:248f3186c666 52
TUATBM 0:248f3186c666 53 /*
TUATBM 0:248f3186c666 54 float g_BefGyro[3] = {0,0,0};
TUATBM 0:248f3186c666 55 double g_HpfGyro[3] = {0,0,0};
TUATBM 0:248f3186c666 56 double g_HpfGyro1[3] = {0,0,0};
TUATBM 0:248f3186c666 57 double g_LpfAccel[3] = {0,0,0};
TUATBM 0:248f3186c666 58 */
TUATBM 0:248f3186c666 59
TUATBM 0:248f3186c666 60 static int jj = 0;
TUATBM 0:248f3186c666 61
TUATBM 0:248f3186c666 62 //IWDG_HandleTypeDef hiwdg;
TUATBM 0:248f3186c666 63
TUATBM 0:248f3186c666 64 void disp_clock(){
TUATBM 0:248f3186c666 65 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
TUATBM 0:248f3186c666 66 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
TUATBM 0:248f3186c666 67 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
TUATBM 0:248f3186c666 68 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
TUATBM 0:248f3186c666 69 pc.printf("\r\n");
TUATBM 0:248f3186c666 70 }
TUATBM 0:248f3186c666 71
TUATBM 0:248f3186c666 72 //サーボ初期化関数
TUATBM 0:248f3186c666 73 void init_Servo()
TUATBM 0:248f3186c666 74 {
TUATBM 0:248f3186c666 75 uint8_t j = 0;
TUATBM 0:248f3186c666 76
TUATBM 0:248f3186c666 77 servo1.period_ms(14);
TUATBM 0:248f3186c666 78 servo1.pulsewidth_us(1500);
TUATBM 0:248f3186c666 79
TUATBM 0:248f3186c666 80 servo2.period_ms(14);
TUATBM 0:248f3186c666 81 servo2.pulsewidth_us(1500);
TUATBM 0:248f3186c666 82
TUATBM 0:248f3186c666 83 servo3.period_ms(14);
TUATBM 0:248f3186c666 84 servo3.pulsewidth_us(1000);
TUATBM 0:248f3186c666 85
TUATBM 0:248f3186c666 86 servo4.period_ms(14);
TUATBM 0:248f3186c666 87 servo4.pulsewidth_us(1500);
TUATBM 0:248f3186c666 88
TUATBM 0:248f3186c666 89 servo5.period_ms(14);
TUATBM 0:248f3186c666 90 servo5.pulsewidth_us(1500);
TUATBM 0:248f3186c666 91
TUATBM 0:248f3186c666 92 servo6.period_ms(14);
TUATBM 0:248f3186c666 93 servo6.pulsewidth_us(1500);
TUATBM 0:248f3186c666 94
TUATBM 0:248f3186c666 95 servo7.period_ms(14);
TUATBM 0:248f3186c666 96 servo7.pulsewidth_us(1500);
TUATBM 0:248f3186c666 97
TUATBM 0:248f3186c666 98 servo8.period_ms(14);
TUATBM 0:248f3186c666 99 servo8.pulsewidth_us(1500);
TUATBM 0:248f3186c666 100
TUATBM 0:248f3186c666 101 for (j=0; j<8; j++) {pwm[j] = 1500;}
TUATBM 0:248f3186c666 102 pc.printf("servo initialized\r\n");
TUATBM 0:248f3186c666 103 }
TUATBM 0:248f3186c666 104
TUATBM 0:248f3186c666 105 //---setall_servo---
TUATBM 0:248f3186c666 106 //pwmをサーボに出力する関数。
TUATBM 0:248f3186c666 107 void setall_servo()
TUATBM 0:248f3186c666 108 {
TUATBM 0:248f3186c666 109 uint8_t j = 0;
TUATBM 0:248f3186c666 110
TUATBM 0:248f3186c666 111 //if(failsafe_status == SBUS_SIGNAL_OK){
TUATBM 0:248f3186c666 112 servo1.pulsewidth_us(pwm[0]);
TUATBM 0:248f3186c666 113 servo2.pulsewidth_us(pwm[1]);
TUATBM 0:248f3186c666 114 servo3.pulsewidth_us(pwm[2]);
TUATBM 0:248f3186c666 115 servo4.pulsewidth_us(pwm[3]);
TUATBM 0:248f3186c666 116 servo5.pulsewidth_us(pwm[4]);
TUATBM 0:248f3186c666 117 servo6.pulsewidth_us(pwm[5]);
TUATBM 0:248f3186c666 118 servo7.pulsewidth_us(pwm[6]);
TUATBM 0:248f3186c666 119 servo8.pulsewidth_us(pwm[7]);
TUATBM 0:248f3186c666 120 /* }else{
TUATBM 0:248f3186c666 121 servo1.pulsewidth_us(1000);
TUATBM 0:248f3186c666 122 servo2.pulsewidth_us(1000);
TUATBM 0:248f3186c666 123 servo3.pulsewidth_us(1000);
TUATBM 0:248f3186c666 124 servo4.pulsewidth_us(1000);
TUATBM 0:248f3186c666 125 servo5.pulsewidth_us(1000);
TUATBM 0:248f3186c666 126 servo6.pulsewidth_us(1000);
TUATBM 0:248f3186c666 127 servo7.pulsewidth_us(1000);
TUATBM 0:248f3186c666 128 servo8.pulsewidth_us(1000);
TUATBM 0:248f3186c666 129 }*/
TUATBM 0:248f3186c666 130
TUATBM 0:248f3186c666 131 for(j=0;j<8;j++){
TUATBM 0:248f3186c666 132 pc.printf("ch%d=%d ", j+1, pwm[j]);
TUATBM 0:248f3186c666 133 }
TUATBM 0:248f3186c666 134 //pc.printf("time = %d", frq.read_ms());
TUATBM 0:248f3186c666 135 pc.printf("\r\n");
TUATBM 0:248f3186c666 136
TUATBM 0:248f3186c666 137 //frq.reset();
TUATBM 0:248f3186c666 138
TUATBM 0:248f3186c666 139 }
TUATBM 0:248f3186c666 140
TUATBM 0:248f3186c666 141
TUATBM 0:248f3186c666 142 //---update_pwm---
TUATBM 0:248f3186c666 143 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
TUATBM 0:248f3186c666 144 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
TUATBM 0:248f3186c666 145 void update_pwm()
TUATBM 0:248f3186c666 146 {
TUATBM 0:248f3186c666 147 uint8_t j = 0;
TUATBM 0:248f3186c666 148
TUATBM 0:248f3186c666 149 if(flg_ch_update == true){
TUATBM 0:248f3186c666 150 switch(update_mode){ //マニュアルモード,自動モード,自動着陸もモードを切替
TUATBM 0:248f3186c666 151 case UPD_MANUAL: //マニュアルモード
TUATBM 0:248f3186c666 152 for(j=0;j<8;j++){
TUATBM 0:248f3186c666 153 pwm[j] = (channels[j]>>1) + 1000;
TUATBM 0:248f3186c666 154 }
TUATBM 0:248f3186c666 155 oldTHL = pwm[2];
TUATBM 0:248f3186c666 156 jj=0;
TUATBM 0:248f3186c666 157 //pc.printf("update_manual\r\n");
TUATBM 0:248f3186c666 158 break;
TUATBM 0:248f3186c666 159
TUATBM 0:248f3186c666 160 case UPD_AUTO: //自動モード
TUATBM 0:248f3186c666 161 pwm[0] = (channels[0]>>1) + 1000;
TUATBM 0:248f3186c666 162 pwm[1] = Auto_ELE;
TUATBM 0:248f3186c666 163 pwm[2] = Auto_THR;
TUATBM 0:248f3186c666 164 pwm[3] = Auto_RUD;
TUATBM 0:248f3186c666 165 pwm[4] = (channels[4]>>1) + 1000;
TUATBM 0:248f3186c666 166 pwm[5] = (channels[5]>>1) + 1000;
TUATBM 0:248f3186c666 167 pwm[6] = (channels[6]>>1) + 1000;
TUATBM 0:248f3186c666 168 pwm[7] = (channels[7]>>1) + 1000;
TUATBM 0:248f3186c666 169
TUATBM 0:248f3186c666 170 //pc.printf("update_auto\r\n");
TUATBM 0:248f3186c666 171 break;
TUATBM 0:248f3186c666 172
TUATBM 0:248f3186c666 173 case UPD_LANDING: //自動着陸モード
TUATBM 0:248f3186c666 174 pwm[0] = (channels[0]>>1) + 1000;
TUATBM 0:248f3186c666 175 pwm[1] = Auto_ELE;
TUATBM 0:248f3186c666 176 pwm[2] = (channels[2]>>1) + 1000;
TUATBM 0:248f3186c666 177 pwm[3] = (channels[3]>>1) + 1000;
TUATBM 0:248f3186c666 178 pwm[4] = (channels[4]>>1) + 1000;
TUATBM 0:248f3186c666 179 pwm[5] = (channels[5]>>1) + 1000;
TUATBM 0:248f3186c666 180 pwm[6] = (channels[6]>>1) + 1000;
TUATBM 0:248f3186c666 181 pwm[7] = (channels[7]>>1) + 1000;
TUATBM 0:248f3186c666 182 break;
TUATBM 0:248f3186c666 183
TUATBM 0:248f3186c666 184 default: //デフォはマニュアルモード
TUATBM 0:248f3186c666 185 for(j=0;j<8;j++){
TUATBM 0:248f3186c666 186 pwm[j] = (channels[j]>>1) + 1000;
TUATBM 0:248f3186c666 187 }
TUATBM 0:248f3186c666 188
TUATBM 0:248f3186c666 189 jj=0;
TUATBM 0:248f3186c666 190 //pc.printf("update_manual\r\n");
TUATBM 0:248f3186c666 191 break;
TUATBM 0:248f3186c666 192 }
TUATBM 0:248f3186c666 193 }else{
TUATBM 0:248f3186c666 194 pc.printf("0\r\n");
TUATBM 0:248f3186c666 195 }
TUATBM 0:248f3186c666 196 flg_ch_update = false;
TUATBM 0:248f3186c666 197 ///oldTHL = pwm[2];
TUATBM 0:248f3186c666 198 setall_servo();
TUATBM 0:248f3186c666 199 }
TUATBM 0:248f3186c666 200
TUATBM 0:248f3186c666 201 /*void update_auto()
TUATBM 0:248f3186c666 202 {
TUATBM 0:248f3186c666 203 pwm[0] = (channels[0]>>1) + 1000;
TUATBM 0:248f3186c666 204 pwm[1] = Auto_ELE;
TUATBM 0:248f3186c666 205 pwm[2] = Auto_THR;
TUATBM 0:248f3186c666 206 pwm[3] = Auto_RUD;
TUATBM 0:248f3186c666 207 pwm[4] = (channels[4]>>1) + 1000;
TUATBM 0:248f3186c666 208 pwm[5] = (channels[5]>>1) + 1000;
TUATBM 0:248f3186c666 209 pwm[6] = (channels[6]>>1) + 1000;
TUATBM 0:248f3186c666 210 pwm[7] = (channels[7]>>1) + 1000;
TUATBM 0:248f3186c666 211
TUATBM 0:248f3186c666 212 setall_servo();
TUATBM 0:248f3186c666 213 //pc.printf("update_auto\r\n");
TUATBM 0:248f3186c666 214 }*/
TUATBM 0:248f3186c666 215
TUATBM 0:248f3186c666 216 //---update_Input---
TUATBM 0:248f3186c666 217 //catch_sbusで取得したSbusの生データから各チャンネルのpwmを抽出する。
TUATBM 0:248f3186c666 218 void update_Input()
TUATBM 0:248f3186c666 219 {
TUATBM 0:248f3186c666 220 uint8_t i = 0;
TUATBM 0:248f3186c666 221
TUATBM 0:248f3186c666 222 /* for (i=0; i<25; i++){
TUATBM 0:248f3186c666 223 pc.printf("%x ", buf_sbus[i]);
TUATBM 0:248f3186c666 224 }pc.printf("\n");
TUATBM 0:248f3186c666 225 */
TUATBM 0:248f3186c666 226 // clear channels[]
TUATBM 0:248f3186c666 227 for (i=0; i<18; i++) {channels[i] = 0;}
TUATBM 0:248f3186c666 228
TUATBM 0:248f3186c666 229 // reset counters
TUATBM 0:248f3186c666 230 uint8_t byte_in_sbus = 1;
TUATBM 0:248f3186c666 231 uint8_t bit_in_sbus = 0;
TUATBM 0:248f3186c666 232 uint8_t ch = 0;
TUATBM 0:248f3186c666 233 uint8_t bit_in_channel = 0;
TUATBM 0:248f3186c666 234
TUATBM 0:248f3186c666 235 // process actual sbus data
TUATBM 0:248f3186c666 236 for (i=0; i<176; i++) {
TUATBM 0:248f3186c666 237 if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) {
TUATBM 0:248f3186c666 238 channels[ch] |= (1<<bit_in_channel);
TUATBM 0:248f3186c666 239 }
TUATBM 0:248f3186c666 240 bit_in_sbus++;
TUATBM 0:248f3186c666 241 bit_in_channel++;
TUATBM 0:248f3186c666 242
TUATBM 0:248f3186c666 243 if (bit_in_sbus == 8) {
TUATBM 0:248f3186c666 244 bit_in_sbus =0;
TUATBM 0:248f3186c666 245 byte_in_sbus++;
TUATBM 0:248f3186c666 246 }
TUATBM 0:248f3186c666 247 if (bit_in_channel == 11) {
TUATBM 0:248f3186c666 248 bit_in_channel =0;
TUATBM 0:248f3186c666 249 ch++;
TUATBM 0:248f3186c666 250 }
TUATBM 0:248f3186c666 251 }
TUATBM 0:248f3186c666 252 // DigiChannel 1
TUATBM 0:248f3186c666 253 if (buf_sbus[23] & (1<<0)) {
TUATBM 0:248f3186c666 254 channels[16] = 1;
TUATBM 0:248f3186c666 255 }else{
TUATBM 0:248f3186c666 256 channels[16] = 0;
TUATBM 0:248f3186c666 257 }
TUATBM 0:248f3186c666 258 // DigiChannel 2
TUATBM 0:248f3186c666 259 if (buf_sbus[23] & (1<<1)) {
TUATBM 0:248f3186c666 260 channels[17] = 1;
TUATBM 0:248f3186c666 261 }else{
TUATBM 0:248f3186c666 262 channels[17] = 0;
TUATBM 0:248f3186c666 263 }
TUATBM 0:248f3186c666 264 // Failsafe
TUATBM 0:248f3186c666 265 failsafe_status = SBUS_SIGNAL_OK;
TUATBM 0:248f3186c666 266 if (buf_sbus[23] & (1<<2)) {
TUATBM 0:248f3186c666 267 failsafe_status = SBUS_SIGNAL_LOST;
TUATBM 0:248f3186c666 268 }
TUATBM 0:248f3186c666 269 if (buf_sbus[23] & (1<<3)) {
TUATBM 0:248f3186c666 270 failsafe_status = SBUS_SIGNAL_FAILSAFE;
TUATBM 0:248f3186c666 271 }
TUATBM 0:248f3186c666 272 //channels[i] = channels[i]>>1;
TUATBM 0:248f3186c666 273 for (i=0; i<25; i++) {buf_sbus[i] = 0;}
TUATBM 0:248f3186c666 274 cnt_sbus = 0;
TUATBM 0:248f3186c666 275 flg_ch_update = true;
TUATBM 0:248f3186c666 276
TUATBM 0:248f3186c666 277 update_pwm();
TUATBM 0:248f3186c666 278
TUATBM 0:248f3186c666 279
TUATBM 0:248f3186c666 280 }
TUATBM 0:248f3186c666 281
TUATBM 0:248f3186c666 282 //---catch_sbus---
TUATBM 0:248f3186c666 283 //受信機から送られてくるSBus信号(シリアル)を取得する関数
TUATBM 0:248f3186c666 284 void catch_sbus()
TUATBM 0:248f3186c666 285 {
TUATBM 0:248f3186c666 286 buf_sbus[cnt_sbus] = sbus_.getc();
TUATBM 0:248f3186c666 287 if(buf_sbus[0] == 0x0f)
TUATBM 0:248f3186c666 288 {
TUATBM 0:248f3186c666 289 cnt_sbus ++;
TUATBM 0:248f3186c666 290 }
TUATBM 0:248f3186c666 291 if(cnt_sbus >=25){
TUATBM 0:248f3186c666 292 if(buf_sbus[24] == 0x00){
TUATBM 0:248f3186c666 293 update_Input(); //update_Input実行
TUATBM 0:248f3186c666 294 }else{
TUATBM 0:248f3186c666 295 cnt_sbus = 0;
TUATBM 0:248f3186c666 296 }
TUATBM 0:248f3186c666 297 }
TUATBM 0:248f3186c666 298 return;
TUATBM 0:248f3186c666 299 }
TUATBM 0:248f3186c666 300
TUATBM 0:248f3186c666 301 //---init_sbus---
TUATBM 0:248f3186c666 302 //Sbusを初期化する関数
TUATBM 0:248f3186c666 303 void init_sbus(){
TUATBM 0:248f3186c666 304 // Set Baudrate
TUATBM 0:248f3186c666 305 sbus_.baud(100000);
TUATBM 0:248f3186c666 306 // Set Datalenght & Frame
TUATBM 0:248f3186c666 307 sbus_.format(8, Serial::Even, 2);
TUATBM 0:248f3186c666 308 //start sbus irq
TUATBM 0:248f3186c666 309 sbus_.attach(&catch_sbus, RawSerial::RxIrq);
TUATBM 0:248f3186c666 310 }
TUATBM 0:248f3186c666 311
TUATBM 0:248f3186c666 312 //---CheckSW_up---
TUATBM 0:248f3186c666 313 //プロポのスイッチがonかoffか返す関数
TUATBM 0:248f3186c666 314 //ch 1~8
TUATBM 0:248f3186c666 315 bool CheckSW_up(int8_t ch)
TUATBM 0:248f3186c666 316 {
TUATBM 0:248f3186c666 317 uint16_t checkpwm;
TUATBM 0:248f3186c666 318 checkpwm = (channels[ch-1]>>1) + 1000;
TUATBM 0:248f3186c666 319 //pc.printf("ch%d=%d ", ch, checkpwm);
TUATBM 0:248f3186c666 320 if(SWITCH_CHECK > checkpwm){
TUATBM 0:248f3186c666 321 return true;
TUATBM 0:248f3186c666 322 }else{
TUATBM 0:248f3186c666 323 return false;
TUATBM 0:248f3186c666 324 }
TUATBM 0:248f3186c666 325 }
TUATBM 0:248f3186c666 326
TUATBM 0:248f3186c666 327
TUATBM 0:248f3186c666 328
TUATBM 0:248f3186c666 329 //---sensing---
TUATBM 0:248f3186c666 330 //センサーの値を読み込み、各種フィルタをかける(認知)
TUATBM 0:248f3186c666 331 void sensing()
TUATBM 0:248f3186c666 332 {
TUATBM 0:248f3186c666 333 // If intPin goes high, all data registers have new data
TUATBM 0:248f3186c666 334 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
TUATBM 0:248f3186c666 335
TUATBM 0:248f3186c666 336 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
TUATBM 0:248f3186c666 337 // Now we'll calculate the accleration value into actual g's
TUATBM 0:248f3186c666 338 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
TUATBM 0:248f3186c666 339 ay = (float)accelCount[1]*aRes - accelBias[1];
TUATBM 0:248f3186c666 340 az = (float)accelCount[2]*aRes - accelBias[2];
TUATBM 0:248f3186c666 341
TUATBM 0:248f3186c666 342 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
TUATBM 0:248f3186c666 343 // Calculate the gyro value into actual degrees per second
TUATBM 0:248f3186c666 344 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
TUATBM 0:248f3186c666 345 gy = (float)gyroCount[1]*gRes - gyroBias[1];
TUATBM 0:248f3186c666 346 gz = (float)gyroCount[2]*gRes - gyroBias[2];
TUATBM 0:248f3186c666 347
TUATBM 0:248f3186c666 348 mpu9250.readMagData(magCount); // Read the x/y/z adc values
TUATBM 0:248f3186c666 349 // Calculate the magnetometer values in milliGauss
TUATBM 0:248f3186c666 350 // Include factory calibration per data sheet and user environmental corrections
TUATBM 0:248f3186c666 351 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
TUATBM 0:248f3186c666 352 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
TUATBM 0:248f3186c666 353 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
TUATBM 0:248f3186c666 354 }
TUATBM 0:248f3186c666 355
TUATBM 0:248f3186c666 356 Now = t.read_us();
TUATBM 0:248f3186c666 357 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
TUATBM 0:248f3186c666 358 lastUpdate = Now;
TUATBM 0:248f3186c666 359
TUATBM 0:248f3186c666 360 sum += deltat;
TUATBM 0:248f3186c666 361 sumCount++;
TUATBM 0:248f3186c666 362
TUATBM 0:248f3186c666 363 // if(lastUpdate - firstUpdate > 10000000.0f) {
TUATBM 0:248f3186c666 364 // beta = 0.04; // decrease filter gain after stabilized
TUATBM 0:248f3186c666 365 // zeta = 0.015; // increasey bias drift gain after stabilized
TUATBM 0:248f3186c666 366 // }
TUATBM 0:248f3186c666 367
TUATBM 0:248f3186c666 368 // Pass gyro rate as rad/s
TUATBM 0:248f3186c666 369 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
TUATBM 0:248f3186c666 370 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
TUATBM 0:248f3186c666 371
TUATBM 0:248f3186c666 372 /* Auto_Loop()で50ms待ってるから要らないと思う
TUATBM 0:248f3186c666 373 // Serial print and/or display at 0.5 s rate independent of data rates
TUATBM 0:248f3186c666 374 delt_t = t.read_ms() - count;
TUATBM 0:248f3186c666 375 if (delt_t > 500) { // update LCD once per half-second independent of read rate
TUATBM 0:248f3186c666 376 */
TUATBM 0:248f3186c666 377
TUATBM 0:248f3186c666 378 //pc.printf("ax = %f", 1000*ax);
TUATBM 0:248f3186c666 379 //pc.printf(" ay = %f", 1000*ay);
TUATBM 0:248f3186c666 380 //pc.printf(" az = %f mg\n\r", 1000*az);
TUATBM 0:248f3186c666 381
TUATBM 0:248f3186c666 382 //pc.printf("gx = %f", gx);
TUATBM 0:248f3186c666 383 //pc.printf(" gy = %f", gy);
TUATBM 0:248f3186c666 384 //pc.printf(" gz = %f deg/s\n\r", gz);
TUATBM 0:248f3186c666 385
TUATBM 0:248f3186c666 386 //pc.printf("mx = %f,", mx);
TUATBM 0:248f3186c666 387 //pc.printf(" my = %f,", my);
TUATBM 0:248f3186c666 388 //pc.printf(" mz = %f mG\n\r", mz);
TUATBM 0:248f3186c666 389
TUATBM 0:248f3186c666 390 tempCount = mpu9250.readTempData(); // Read the adc values
TUATBM 0:248f3186c666 391 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
TUATBM 0:248f3186c666 392 //pc.printf(" temperature = %f C\n\r", temperature);
TUATBM 0:248f3186c666 393
TUATBM 0:248f3186c666 394 //pc.printf("q0 = %f\n\r", q[0]);
TUATBM 0:248f3186c666 395 //pc.printf("q1 = %f\n\r", q[1]);
TUATBM 0:248f3186c666 396 //pc.printf("q2 = %f\n\r", q[2]);
TUATBM 0:248f3186c666 397 //pc.printf("q3 = %f\n\r", q[3]);
TUATBM 0:248f3186c666 398
TUATBM 0:248f3186c666 399
TUATBM 0:248f3186c666 400 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
TUATBM 0:248f3186c666 401 // In this coordinate system, the positive z-axis is down toward Earth.
TUATBM 0:248f3186c666 402 // 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.
TUATBM 0:248f3186c666 403 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
TUATBM 0:248f3186c666 404 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
TUATBM 0:248f3186c666 405 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
TUATBM 0:248f3186c666 406 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
TUATBM 0:248f3186c666 407 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
TUATBM 0:248f3186c666 408 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
TUATBM 0:248f3186c666 409 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]);
TUATBM 0:248f3186c666 410 roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
TUATBM 0:248f3186c666 411 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]);
TUATBM 0:248f3186c666 412 pitch *= 180.0f / PI;
TUATBM 0:248f3186c666 413 yaw *= 180.0f / PI;
TUATBM 0:248f3186c666 414 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
TUATBM 0:248f3186c666 415 roll *= 180.0f / PI;
TUATBM 0:248f3186c666 416
TUATBM 0:248f3186c666 417 //pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100));
TUATBM 0:248f3186c666 418 //pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll);
TUATBM 0:248f3186c666 419 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
TUATBM 0:248f3186c666 420
TUATBM 0:248f3186c666 421 count = t.read_ms();
TUATBM 0:248f3186c666 422 sum = 0;
TUATBM 0:248f3186c666 423 sumCount = 0;
TUATBM 0:248f3186c666 424 //}
TUATBM 0:248f3186c666 425 }
TUATBM 0:248f3186c666 426
TUATBM 0:248f3186c666 427
TUATBM 0:248f3186c666 428 //---CalibrateCompass---
TUATBM 0:248f3186c666 429 //グラウンドチェック時にスイッチ7だけをonにしておくことで実行 リポ抜いたらやり直ししなくてはならない
TUATBM 0:248f3186c666 430 //使わない予定
TUATBM 0:248f3186c666 431 void CalibrateCompass(void)
TUATBM 0:248f3186c666 432 {
TUATBM 0:248f3186c666 433 led1 = 1;
TUATBM 0:248f3186c666 434 sensing();
TUATBM 0:248f3186c666 435 uint8_t ii;
TUATBM 0:248f3186c666 436 float mxMAX=mx, mxMIN=mx;
TUATBM 0:248f3186c666 437 float myMAX=my, myMIN=my;
TUATBM 0:248f3186c666 438 magbias[0] = 0; //初期化
TUATBM 0:248f3186c666 439 magbias[1] = 0;
TUATBM 0:248f3186c666 440 //magbias[2] = 0;
TUATBM 0:248f3186c666 441 ii=0;
TUATBM 0:248f3186c666 442 while(1){ //キャリブレーション実行 led2が点滅時に機体を一回転させる xy方向のみ
TUATBM 0:248f3186c666 443 sensing();
TUATBM 0:248f3186c666 444 if(mxMAX < mx) mxMAX = mx;
TUATBM 0:248f3186c666 445 if(mxMIN > mx) mxMIN = mx;
TUATBM 0:248f3186c666 446 if(myMAX < my) myMAX = my;
TUATBM 0:248f3186c666 447 if(myMIN > my) myMIN = my;
TUATBM 0:248f3186c666 448 led2 = !led2;
TUATBM 0:248f3186c666 449 ++ii;
TUATBM 0:248f3186c666 450 wait(50);
TUATBM 0:248f3186c666 451
TUATBM 0:248f3186c666 452 if(!CheckSW_up(7)) break; //スイッチ7を下げて終了
TUATBM 0:248f3186c666 453 }
TUATBM 0:248f3186c666 454 led2 = 0;
TUATBM 0:248f3186c666 455 if(ii<20){ //チャンネル7をすぐ切った場合 元に戻す
TUATBM 0:248f3186c666 456 magbias[0] = MAGBIASX;
TUATBM 0:248f3186c666 457 magbias[1] = MAGBIASY;
TUATBM 0:248f3186c666 458 }else{ //しっかりと値を入れた場合
TUATBM 0:248f3186c666 459 magbias[0] = (mxMAX+mxMIN)/2;
TUATBM 0:248f3186c666 460 magbias[1] = (myMAX+myMIN)/2;
TUATBM 0:248f3186c666 461 }
TUATBM 0:248f3186c666 462 led1 = 0;
TUATBM 0:248f3186c666 463 t.reset(); //タイマーをリセットして終了
TUATBM 0:248f3186c666 464 }
TUATBM 0:248f3186c666 465
TUATBM 0:248f3186c666 466
TUATBM 0:248f3186c666 467 //---StabiGyro---
TUATBM 0:248f3186c666 468 //水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
TUATBM 0:248f3186c666 469 //右旋回
TUATBM 0:248f3186c666 470 void StabiGyro()
TUATBM 0:248f3186c666 471 {
TUATBM 0:248f3186c666 472 if(0 /*oldTHL < 1200*/){ //機体審査用
TUATBM 0:248f3186c666 473 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN-gy*RUD_DGN);
TUATBM 0:248f3186c666 474 Auto_ELE = ELE_N + -50 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 475 Auto_THR = 1500;
TUATBM 0:248f3186c666 476 }else{
TUATBM 0:248f3186c666 477 if(jj<=0){ //旋回し始め 15
TUATBM 0:248f3186c666 478 Auto_RUD = RUD_N + -65 + int((-12 -roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5
TUATBM 0:248f3186c666 479 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx
TUATBM 0:248f3186c666 480 Auto_THR = oldTHL + 30;
TUATBM 0:248f3186c666 481 //pc.printf("first%d\r\n",jj);
TUATBM 0:248f3186c666 482 jj++;
TUATBM 0:248f3186c666 483 }else{ //旋回中
TUATBM 0:248f3186c666 484 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
TUATBM 0:248f3186c666 485 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 486 Auto_THR = oldTHL ;
TUATBM 0:248f3186c666 487 }
TUATBM 0:248f3186c666 488 }
TUATBM 0:248f3186c666 489
TUATBM 0:248f3186c666 490 //pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする
TUATBM 0:248f3186c666 491 if(Auto_RUD > RUD_MAX){
TUATBM 0:248f3186c666 492 Auto_RUD = RUD_MAX;
TUATBM 0:248f3186c666 493 }else if(Auto_RUD < RUD_MIN){
TUATBM 0:248f3186c666 494 Auto_RUD = RUD_MIN;
TUATBM 0:248f3186c666 495 }
TUATBM 0:248f3186c666 496 if(Auto_ELE > ELE_MAX){
TUATBM 0:248f3186c666 497 Auto_ELE = ELE_MAX;
TUATBM 0:248f3186c666 498 }else if(Auto_ELE < ELE_MIN){
TUATBM 0:248f3186c666 499 Auto_ELE = ELE_MIN;
TUATBM 0:248f3186c666 500 }
TUATBM 0:248f3186c666 501
TUATBM 0:248f3186c666 502
TUATBM 0:248f3186c666 503
TUATBM 0:248f3186c666 504 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
TUATBM 0:248f3186c666 505 //Thread::wait(G_MS);
TUATBM 0:248f3186c666 506 }
TUATBM 0:248f3186c666 507
TUATBM 0:248f3186c666 508 //---StabiGyro_Mobius---
TUATBM 0:248f3186c666 509 //8の字旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
TUATBM 0:248f3186c666 510 void StabiGyro_Mobius()
TUATBM 0:248f3186c666 511 {
TUATBM 0:248f3186c666 512 if(jj<=15){ //右旋回導入
TUATBM 0:248f3186c666 513 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - (gy + 130)*0.75); //*0.75); //roll -> gy 3.5
TUATBM 0:248f3186c666 514 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - (gx + 30)*0.8); //pitch -> -gx
TUATBM 0:248f3186c666 515 Auto_THR = oldTHL;
TUATBM 0:248f3186c666 516 //pc.printf("first%d\r\n",jj);
TUATBM 0:248f3186c666 517 jj++;
TUATBM 0:248f3186c666 518 }else if((15<jj) && (jj<=70)){ //右旋回途中
TUATBM 0:248f3186c666 519 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
TUATBM 0:248f3186c666 520 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 521 Auto_THR = oldTHL;
TUATBM 0:248f3186c666 522 jj++;
TUATBM 0:248f3186c666 523 }else if((70<jj) && (jj<=75)){ //旋回遷移1
TUATBM 0:248f3186c666 524 Auto_RUD = RUD_N + 100 + int((16 -roll)*RUDDER_GN - (gy-100)*0.75);
TUATBM 0:248f3186c666 525 Auto_ELE = ELE_N + 20 + int((0 -pitch)*ELEVATOR_GN - (gx + 0)*0.6);
TUATBM 0:248f3186c666 526 Auto_THR = THR_N - 100;
TUATBM 0:248f3186c666 527 //pc.printf("first%d\r\n",jj);
TUATBM 0:248f3186c666 528 jj++;
TUATBM 0:248f3186c666 529 }else if((75<jj) && (jj<=90)){ //旋回遷移2
TUATBM 0:248f3186c666 530 Auto_RUD = RUD_N + 65 + int((12-roll)*RUDDER_GN - (gy-100)*0.75); // 105 30
TUATBM 0:248f3186c666 531 Auto_ELE = ELE_N + -15 + int((-5 - pitch)*ELEVATOR_GN - (gx+30)*0.6); //-75 -15
TUATBM 0:248f3186c666 532 Auto_THR = oldTHL - 20; //-20
TUATBM 0:248f3186c666 533 jj++;
TUATBM 0:248f3186c666 534 }else{ //左旋回途中
TUATBM 0:248f3186c666 535 Auto_RUD = RUD_N + 65 + int((12-roll)*RUDDER_GN - gy*RUD_DGN);
TUATBM 0:248f3186c666 536 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 537 Auto_THR = oldTHL;
TUATBM 0:248f3186c666 538 }
TUATBM 0:248f3186c666 539
TUATBM 0:248f3186c666 540 if(Auto_RUD > RUD_MAX){
TUATBM 0:248f3186c666 541 Auto_RUD = RUD_MAX;
TUATBM 0:248f3186c666 542 }else if(Auto_RUD < RUD_MIN){
TUATBM 0:248f3186c666 543 Auto_RUD = RUD_MIN;
TUATBM 0:248f3186c666 544 }
TUATBM 0:248f3186c666 545 if(Auto_ELE > ELE_MAX){
TUATBM 0:248f3186c666 546 Auto_ELE = ELE_MAX;
TUATBM 0:248f3186c666 547 }else if(Auto_ELE < ELE_MIN){
TUATBM 0:248f3186c666 548 Auto_ELE = ELE_MIN;
TUATBM 0:248f3186c666 549 }
TUATBM 0:248f3186c666 550
TUATBM 0:248f3186c666 551 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
TUATBM 0:248f3186c666 552 //Thread::wait(G_MS);
TUATBM 0:248f3186c666 553 }
TUATBM 0:248f3186c666 554
TUATBM 0:248f3186c666 555 //---StabiGyro_Climb---
TUATBM 0:248f3186c666 556 //上昇旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
TUATBM 0:248f3186c666 557 void StabiGyro_Climb()
TUATBM 0:248f3186c666 558 {
TUATBM 0:248f3186c666 559 if(jj<=15){ //右旋回導入
TUATBM 0:248f3186c666 560 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - (gy+130)*RUD_DGN); //*0.75); //roll -> gy 3.5
TUATBM 0:248f3186c666 561 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - (gx+30)*ELE_DGN); //pitch -> -gx
TUATBM 0:248f3186c666 562 Auto_THR = oldTHL;
TUATBM 0:248f3186c666 563 //pc.printf("first%d\r\n",jj);
TUATBM 0:248f3186c666 564 jj++;
TUATBM 0:248f3186c666 565 }else if((15<jj) && (jj<=136)){ //右旋回途中
TUATBM 0:248f3186c666 566 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
TUATBM 0:248f3186c666 567 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 568 Auto_THR = oldTHL;
TUATBM 0:248f3186c666 569 jj++;
TUATBM 0:248f3186c666 570 /*}else if((136<jj) && (jj<=146)){ //上昇遷移
TUATBM 0:248f3186c666 571 Auto_RUD = RUD_N + -65 - int((-12-roll)*RUDDER_GN - gy*0);
TUATBM 0:248f3186c666 572 Auto_ELE = ELE_N + -80 + int((-22-pitch)*ELEVATOR_GN - gx*0);
TUATBM 0:248f3186c666 573 Auto_THR = THR_N + 100;
TUATBM 0:248f3186c666 574 //pc.printf("first%d\r\n",jj);
TUATBM 0:248f3186c666 575 jj++;*/
TUATBM 0:248f3186c666 576 }else if((136<jj) && (jj<=200)){ //上昇中
TUATBM 0:248f3186c666 577 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
TUATBM 0:248f3186c666 578 Auto_ELE = ELE_N + -140 + int((-25 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 579 Auto_THR = oldTHL + 220;
TUATBM 0:248f3186c666 580 jj++;
TUATBM 0:248f3186c666 581 }else{ //水平旋回
TUATBM 0:248f3186c666 582 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
TUATBM 0:248f3186c666 583 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 584 Auto_THR = oldTHL;
TUATBM 0:248f3186c666 585 }
TUATBM 0:248f3186c666 586
TUATBM 0:248f3186c666 587 if(Auto_RUD > RUD_MAX){
TUATBM 0:248f3186c666 588 Auto_RUD = RUD_MAX;
TUATBM 0:248f3186c666 589 }else if(Auto_RUD < RUD_MIN){
TUATBM 0:248f3186c666 590 Auto_RUD = RUD_MIN;
TUATBM 0:248f3186c666 591 }
TUATBM 0:248f3186c666 592 if(Auto_ELE > ELE_MAX){
TUATBM 0:248f3186c666 593 Auto_ELE = ELE_MAX;
TUATBM 0:248f3186c666 594 }else if(Auto_ELE < ELE_MIN){
TUATBM 0:248f3186c666 595 Auto_ELE = ELE_MIN;
TUATBM 0:248f3186c666 596 }
TUATBM 0:248f3186c666 597
TUATBM 0:248f3186c666 598 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
TUATBM 0:248f3186c666 599 //Thread::wait(G_MS);
TUATBM 0:248f3186c666 600 }
TUATBM 0:248f3186c666 601
TUATBM 0:248f3186c666 602 //---StabiGyro_Glide---
TUATBM 0:248f3186c666 603 //自動滑空のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
TUATBM 0:248f3186c666 604 void StabiGyro_Glide()
TUATBM 0:248f3186c666 605 {
TUATBM 0:248f3186c666 606 if(jj<=15){
TUATBM 0:248f3186c666 607 Auto_RUD = RUD_N;
TUATBM 0:248f3186c666 608 Auto_ELE = ELE_N + -5 + int (( - pitch)*ELEVATOR_GN - gx*0);
TUATBM 0:248f3186c666 609 Auto_THR = 1000;
TUATBM 0:248f3186c666 610 //pc.printf("first%d\r\n",jj);
TUATBM 0:248f3186c666 611 jj++;
TUATBM 0:248f3186c666 612 }else{
TUATBM 0:248f3186c666 613 Auto_RUD = RUD_N + -30 + int((-8 - roll)*RUDDER_GN - gy*RUD_DGN);
TUATBM 0:248f3186c666 614 Auto_ELE = ELE_N + -25 + int((-5- pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 615 Auto_THR = 1000; //スロットルoff
TUATBM 0:248f3186c666 616 }
TUATBM 0:248f3186c666 617 if(Auto_RUD > RUD_MAX){
TUATBM 0:248f3186c666 618 Auto_RUD = RUD_MAX;
TUATBM 0:248f3186c666 619 }else if(Auto_RUD < RUD_MIN){
TUATBM 0:248f3186c666 620 Auto_RUD = RUD_MIN;
TUATBM 0:248f3186c666 621 }
TUATBM 0:248f3186c666 622 if(Auto_ELE > ELE_MAX){
TUATBM 0:248f3186c666 623 Auto_ELE = ELE_MAX;
TUATBM 0:248f3186c666 624 }else if(Auto_ELE < ELE_MIN){
TUATBM 0:248f3186c666 625 Auto_ELE = ELE_MIN;
TUATBM 0:248f3186c666 626 }
TUATBM 0:248f3186c666 627
TUATBM 0:248f3186c666 628 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
TUATBM 0:248f3186c666 629 //Thread::wait(G_MS);
TUATBM 0:248f3186c666 630 }
TUATBM 0:248f3186c666 631
TUATBM 0:248f3186c666 632 void StabiGyro_Landing()
TUATBM 0:248f3186c666 633 {
TUATBM 0:248f3186c666 634 if(pwm[0]<1300){
TUATBM 0:248f3186c666 635 Auto_ELE = ELE_N + -10 + int((-5 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 636 Auto_THR = oldTHL;
TUATBM 0:248f3186c666 637 }else if((1300<=pwm[0])&&(pwm[0]<1800)){
TUATBM 0:248f3186c666 638 Auto_ELE = ELE_N + -15 + int((-8 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 639 Auto_THR = oldTHL - 30;
TUATBM 0:248f3186c666 640 }else if(1800<=pwm[0]){
TUATBM 0:248f3186c666 641 Auto_ELE = ELE_N + -25 + int((-12 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
TUATBM 0:248f3186c666 642 Auto_THR = 1000;
TUATBM 0:248f3186c666 643 }
TUATBM 0:248f3186c666 644 }
TUATBM 0:248f3186c666 645
TUATBM 0:248f3186c666 646 void Auto_Loop()
TUATBM 0:248f3186c666 647 {
TUATBM 0:248f3186c666 648 //while(true){
TUATBM 0:248f3186c666 649 //for(k=0;k<100;++k){
TUATBM 0:248f3186c666 650 //sensing(); //センサーの値を読み込む(認知)
TUATBM 0:248f3186c666 651 StabiGyro(); //水平旋回のためのラダー、エレベーターのpwmを計算(判断)
TUATBM 0:248f3186c666 652 update_mode = UPD_AUTO; //オートモード
TUATBM 0:248f3186c666 653 //StabiAccel();
TUATBM 0:248f3186c666 654 //Auto_ELE+=145;
TUATBM 0:248f3186c666 655 //Auto_RUD-=80;
TUATBM 0:248f3186c666 656 //Servos.Auto2Servo();
TUATBM 0:248f3186c666 657 //Thread::wait(50);
TUATBM 0:248f3186c666 658 /*
TUATBM 0:248f3186c666 659 if(!CheckSW_up(7)){
TUATBM 0:248f3186c666 660 pc.printf("go to manual\n");
TUATBM 0:248f3186c666 661 break;
TUATBM 0:248f3186c666 662 }*/
TUATBM 0:248f3186c666 663 //wait_ms(50);
TUATBM 0:248f3186c666 664 //}
TUATBM 0:248f3186c666 665 //k=0; //for debug
TUATBM 0:248f3186c666 666 //pwm[6]=1000;
TUATBM 0:248f3186c666 667 }
TUATBM 0:248f3186c666 668
TUATBM 0:248f3186c666 669 void Auto_Mobius()
TUATBM 0:248f3186c666 670 {
TUATBM 0:248f3186c666 671 //sensing();
TUATBM 0:248f3186c666 672 StabiGyro_Mobius();
TUATBM 0:248f3186c666 673 update_mode = UPD_AUTO;
TUATBM 0:248f3186c666 674 //wait_ms(50);
TUATBM 0:248f3186c666 675 }
TUATBM 0:248f3186c666 676
TUATBM 0:248f3186c666 677 void Auto_Climb()
TUATBM 0:248f3186c666 678 {
TUATBM 0:248f3186c666 679 //sensing();
TUATBM 0:248f3186c666 680 StabiGyro_Climb();
TUATBM 0:248f3186c666 681 update_mode = UPD_AUTO;
TUATBM 0:248f3186c666 682 //wait_ms(50);
TUATBM 0:248f3186c666 683 }
TUATBM 0:248f3186c666 684
TUATBM 0:248f3186c666 685 void Auto_Glide()
TUATBM 0:248f3186c666 686 {
TUATBM 0:248f3186c666 687 //sensing();
TUATBM 0:248f3186c666 688 StabiGyro_Glide();
TUATBM 0:248f3186c666 689 update_mode = UPD_AUTO;
TUATBM 0:248f3186c666 690 //wait_ms(50);
TUATBM 0:248f3186c666 691 }
TUATBM 0:248f3186c666 692
TUATBM 0:248f3186c666 693 void Auto_Landing()
TUATBM 0:248f3186c666 694 {
TUATBM 0:248f3186c666 695 //sensing();
TUATBM 0:248f3186c666 696 StabiGyro_Landing();
TUATBM 0:248f3186c666 697 update_mode = UPD_MANUAL;
TUATBM 0:248f3186c666 698 //wait_ms(50);
TUATBM 0:248f3186c666 699 }
TUATBM 0:248f3186c666 700
TUATBM 0:248f3186c666 701 int main()
TUATBM 0:248f3186c666 702 {
TUATBM 0:248f3186c666 703 //初期化開始
TUATBM 0:248f3186c666 704 led1 = 1;
TUATBM 0:248f3186c666 705 led2 = 1;
TUATBM 0:248f3186c666 706 led3 = 0;
TUATBM 0:248f3186c666 707 led4 = 0;
TUATBM 0:248f3186c666 708
TUATBM 0:248f3186c666 709 //SBus用バッファ初期化
TUATBM 0:248f3186c666 710 int count_initsbus;
TUATBM 0:248f3186c666 711 for (count_initsbus = 0; count_initsbus < 25; count_initsbus++) {buf_sbus[count_initsbus] = 0;}
TUATBM 0:248f3186c666 712
TUATBM 0:248f3186c666 713 //pc.baud(115200); //パソコン通信用シリアルのボードレート
TUATBM 0:248f3186c666 714
TUATBM 0:248f3186c666 715 //frq.start();
TUATBM 0:248f3186c666 716 init_Servo();
TUATBM 0:248f3186c666 717 init_sbus();
TUATBM 0:248f3186c666 718 disp_clock();
TUATBM 0:248f3186c666 719
TUATBM 0:248f3186c666 720 //---9軸センサー(MPU9250)初期化---
TUATBM 0:248f3186c666 721
TUATBM 0:248f3186c666 722 //Set up I2C
TUATBM 0:248f3186c666 723 i2c.frequency(400000); // use fast (400 kHz) I2C
TUATBM 0:248f3186c666 724 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
TUATBM 0:248f3186c666 725 t.start();
TUATBM 0:248f3186c666 726
TUATBM 0:248f3186c666 727 // Read the WHO_AM_I register, this is a good test of communication
TUATBM 0:248f3186c666 728 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
TUATBM 0:248f3186c666 729 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
TUATBM 0:248f3186c666 730
TUATBM 0:248f3186c666 731 if (whoami == 0x71){ // WHO_AM_I should always be 0x68
TUATBM 0:248f3186c666 732 pc.printf("MPU9250 is online...\n\r");
TUATBM 0:248f3186c666 733 //wait(1);
TUATBM 0:248f3186c666 734
TUATBM 0:248f3186c666 735 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
TUATBM 0:248f3186c666 736 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
TUATBM 0:248f3186c666 737 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
TUATBM 0:248f3186c666 738 //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
TUATBM 0:248f3186c666 739 //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
TUATBM 0:248f3186c666 740 //pc.printf("x accel bias = %f\n\r", accelBias[0]);
TUATBM 0:248f3186c666 741 //pc.printf("y accel bias = %f\n\r", accelBias[1]);
TUATBM 0:248f3186c666 742 //pc.printf("z accel bias = %f\n\r", accelBias[2]);
TUATBM 0:248f3186c666 743 wait(1);
TUATBM 0:248f3186c666 744
TUATBM 0:248f3186c666 745 mpu9250.initMPU9250();
TUATBM 0:248f3186c666 746 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
TUATBM 0:248f3186c666 747 mpu9250.initAK8963(magCalibration);
TUATBM 0:248f3186c666 748 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
TUATBM 0:248f3186c666 749 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
TUATBM 0:248f3186c666 750 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
TUATBM 0:248f3186c666 751 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
TUATBM 0:248f3186c666 752 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
TUATBM 0:248f3186c666 753 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
TUATBM 0:248f3186c666 754 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
TUATBM 0:248f3186c666 755 wait(1);
TUATBM 0:248f3186c666 756
TUATBM 0:248f3186c666 757 }else{
TUATBM 0:248f3186c666 758 pc.printf("Could not connect to MPU9250: \n\r");
TUATBM 0:248f3186c666 759 pc.printf("%#x \n", whoami);
TUATBM 0:248f3186c666 760 while(1) ; // Loop forever if communication doesn't happen
TUATBM 0:248f3186c666 761 }
TUATBM 0:248f3186c666 762
TUATBM 0:248f3186c666 763 mpu9250.getAres(); // Get accelerometer sensitivity
TUATBM 0:248f3186c666 764 mpu9250.getGres(); // Get gyro sensitivity
TUATBM 0:248f3186c666 765 mpu9250.getMres(); // Get magnetometer sensitivity
TUATBM 0:248f3186c666 766 //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
TUATBM 0:248f3186c666 767 //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
TUATBM 0:248f3186c666 768 //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
TUATBM 0:248f3186c666 769 magbias[0] = MAGBIASX; //+470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
TUATBM 0:248f3186c666 770 magbias[1] = MAGBIASY; //+120.; // User environmental y-axis correction in milliGauss
TUATBM 0:248f3186c666 771 magbias[2] = MAGBIASZ; //+125.; // User environmental z-axis correction in milliGauss
TUATBM 0:248f3186c666 772
TUATBM 0:248f3186c666 773 t.start();
TUATBM 0:248f3186c666 774 pc.printf("mpu9250 initialized\r\n");
TUATBM 0:248f3186c666 775
TUATBM 0:248f3186c666 776 //---9軸センサー(MPU9250)初期化終わり---
TUATBM 0:248f3186c666 777
TUATBM 0:248f3186c666 778 led1 = 0;
TUATBM 0:248f3186c666 779 led2 = 0;
TUATBM 0:248f3186c666 780 led3 = 0;
TUATBM 0:248f3186c666 781 led4 = 0;
TUATBM 0:248f3186c666 782
TUATBM 0:248f3186c666 783 pc.printf("All initialized\r\n");
TUATBM 0:248f3186c666 784
TUATBM 0:248f3186c666 785
TUATBM 0:248f3186c666 786 while (true) { //無限ループ
TUATBM 0:248f3186c666 787 //for(i=0;i<8;i++){
TUATBM 0:248f3186c666 788 //pc.printf("%x ", buf_sbus[i]);
TUATBM 0:248f3186c666 789 //pc.printf("ch%d=%d ", i+1, channels[i]);
TUATBM 0:248f3186c666 790 //}
TUATBM 0:248f3186c666 791 //pc.printf("\n");
TUATBM 0:248f3186c666 792 sensing();
TUATBM 0:248f3186c666 793 wait_ms(50);
TUATBM 0:248f3186c666 794
TUATBM 0:248f3186c666 795
TUATBM 0:248f3186c666 796 switch(OperationMode){ //変数OperationModeの値で場合分け
TUATBM 0:248f3186c666 797 case GROUNDCHECK: //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行)
TUATBM 0:248f3186c666 798 if(!CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7,8が両方offなら
TUATBM 0:248f3186c666 799 OperationMode=AUTOLOOP; //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入
TUATBM 0:248f3186c666 800 pc.printf("go to autoloop\r\n");
TUATBM 0:248f3186c666 801 led1 = 0;
TUATBM 0:248f3186c666 802 led2 = 0;
TUATBM 0:248f3186c666 803 }else{
TUATBM 0:248f3186c666 804 led1 = 1;
TUATBM 0:248f3186c666 805 led2 = 1;
TUATBM 0:248f3186c666 806 }
TUATBM 0:248f3186c666 807 //変数update_modeはSBus(=プロポ＝操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数
TUATBM 0:248f3186c666 808 update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す)
TUATBM 0:248f3186c666 809 // pc.printf("groundcheck mode\r\n");
TUATBM 0:248f3186c666 810
TUATBM 0:248f3186c666 811 break;
TUATBM 0:248f3186c666 812 case AUTOLOOP: //水平旋回モード
TUATBM 0:248f3186c666 813 //t.reset();
TUATBM 0:248f3186c666 814 if(CheckSW_up(7)){ //チャンネル7がonなら
TUATBM 0:248f3186c666 815 //t.reset();
TUATBM 0:248f3186c666 816 Auto_Loop(); //関数Auto_Loop実行
TUATBM 0:248f3186c666 817 led1 = 1;
TUATBM 0:248f3186c666 818 led2 = 0;
TUATBM 0:248f3186c666 819 //Auto_Loop();
TUATBM 0:248f3186c666 820 //pc.printf("Auto Loop Now\r\n");
TUATBM 0:248f3186c666 821 //autoloop.set_priority(osPriorityAboveNormal);
TUATBM 0:248f3186c666 822 /*
TUATBM 0:248f3186c666 823 while(true){
TUATBM 0:248f3186c666 824 if(!CheckSW_up(7)){
TUATBM 0:248f3186c666 825 autoloop.terminate();
TUATBM 0:248f3186c666 826 break;
TUATBM 0:248f3186c666 827 }
TUATBM 0:248f3186c666 828 }
TUATBM 0:248f3186c666 829 */
TUATBM 0:248f3186c666 830 }else{
TUATBM 0:248f3186c666 831 update_mode = UPD_MANUAL;
TUATBM 0:248f3186c666 832 //pc.printf("manual Now\r\n");
TUATBM 0:248f3186c666 833 led1 = 0;
TUATBM 0:248f3186c666 834 led2 = 0;
TUATBM 0:248f3186c666 835
TUATBM 0:248f3186c666 836 /*debug*/
TUATBM 0:248f3186c666 837 //k++;
TUATBM 0:248f3186c666 838 //wait(50);
TUATBM 0:248f3186c666 839 //if(k>100) pwm[6]=2000;
TUATBM 0:248f3186c666 840 }
TUATBM 0:248f3186c666 841
TUATBM 0:248f3186c666 842 if(!CheckSW_up(7)&&CheckSW_up(8)){ //チャンネル7がoff、8がonなら
TUATBM 0:248f3186c666 843 OperationMode=AUTOMOBIUS;
TUATBM 0:248f3186c666 844 pc.printf("go to auto8\r\n");
TUATBM 0:248f3186c666 845 }
TUATBM 0:248f3186c666 846 break;
TUATBM 0:248f3186c666 847 case AUTOMOBIUS: //8の字旋回モード
TUATBM 0:248f3186c666 848 if(CheckSW_up(7)){
TUATBM 0:248f3186c666 849 Auto_Mobius();
TUATBM 0:248f3186c666 850 led1 = 1;
TUATBM 0:248f3186c666 851 led2 = 1;
TUATBM 0:248f3186c666 852 //pc.printf("Auto Mobius Now\r\n");
TUATBM 0:248f3186c666 853 }else{
TUATBM 0:248f3186c666 854 update_mode = UPD_MANUAL;
TUATBM 0:248f3186c666 855 //pc.printf("manual Now\r\n");
TUATBM 0:248f3186c666 856 led1 = 0;
TUATBM 0:248f3186c666 857 led2 = 1;
TUATBM 0:248f3186c666 858 }
TUATBM 0:248f3186c666 859
TUATBM 0:248f3186c666 860 if(!CheckSW_up(7)&&!CheckSW_up(8)){
TUATBM 0:248f3186c666 861 OperationMode=AUTOCLIMB;
TUATBM 0:248f3186c666 862 pc.printf("go to autoclimb\r\n");
TUATBM 0:248f3186c666 863 }
TUATBM 0:248f3186c666 864 break;
TUATBM 0:248f3186c666 865 case AUTOCLIMB: //上昇旋回モード
TUATBM 0:248f3186c666 866 if(CheckSW_up(7)){
TUATBM 0:248f3186c666 867 Auto_Climb();
TUATBM 0:248f3186c666 868 led1 = 1;
TUATBM 0:248f3186c666 869 led2 = 0;
TUATBM 0:248f3186c666 870 //pc.printf("Auto Climb Now\r\n");
TUATBM 0:248f3186c666 871 }else{
TUATBM 0:248f3186c666 872 update_mode = UPD_MANUAL;
TUATBM 0:248f3186c666 873 //pc.printf("manual Now\r\n");
TUATBM 0:248f3186c666 874 led1 = 0;
TUATBM 0:248f3186c666 875 led2 = 0;
TUATBM 0:248f3186c666 876 }
TUATBM 0:248f3186c666 877
TUATBM 0:248f3186c666 878 if(!CheckSW_up(7)&&CheckSW_up(8)){
TUATBM 0:248f3186c666 879 OperationMode=AUTOGLIDE;
TUATBM 0:248f3186c666 880 pc.printf("go to manual\r\n");
TUATBM 0:248f3186c666 881 }
TUATBM 0:248f3186c666 882 break;
TUATBM 0:248f3186c666 883
TUATBM 0:248f3186c666 884 case AUTOGLIDE: //自動滑空モード
TUATBM 0:248f3186c666 885 if(CheckSW_up(7)){
TUATBM 0:248f3186c666 886 Auto_Glide();
TUATBM 0:248f3186c666 887 led1 = 1;
TUATBM 0:248f3186c666 888 led2 = 1;
TUATBM 0:248f3186c666 889 //pc.printf("Auto Glide Now\r\n");
TUATBM 0:248f3186c666 890 }else{
TUATBM 0:248f3186c666 891 update_mode = UPD_MANUAL;
TUATBM 0:248f3186c666 892 //pc.printf("manual Now\r\n");
TUATBM 0:248f3186c666 893 led1 = 0;
TUATBM 0:248f3186c666 894 led2 = 1;
TUATBM 0:248f3186c666 895 }
TUATBM 0:248f3186c666 896
TUATBM 0:248f3186c666 897 if(!CheckSW_up(7)&&!CheckSW_up(8)){
TUATBM 0:248f3186c666 898 OperationMode=AUTOLANDING;
TUATBM 0:248f3186c666 899 pc.printf("go to autoloop\r\n");
TUATBM 0:248f3186c666 900 }
TUATBM 0:248f3186c666 901 break;
TUATBM 0:248f3186c666 902
TUATBM 0:248f3186c666 903 case AUTOLANDING:
TUATBM 0:248f3186c666 904 led3 = 1;
TUATBM 0:248f3186c666 905 led4 = 1; //赤外線
TUATBM 0:248f3186c666 906 if(CheckSW_up(7)){
TUATBM 0:248f3186c666 907 Auto_Landing();
TUATBM 0:248f3186c666 908 led1 = 1;
TUATBM 0:248f3186c666 909 led2 = 0;
TUATBM 0:248f3186c666 910 //pc.printf("Auto Landing Now\r\n");
TUATBM 0:248f3186c666 911 }else{
TUATBM 0:248f3186c666 912 update_mode = UPD_MANUAL;
TUATBM 0:248f3186c666 913 //pc.printf("manual Now\r\n");
TUATBM 0:248f3186c666 914 led1 = 0;
TUATBM 0:248f3186c666 915 led2 = 0;
TUATBM 0:248f3186c666 916 }
TUATBM 0:248f3186c666 917
TUATBM 0:248f3186c666 918 if(!CheckSW_up(7)&&CheckSW_up(8)){
TUATBM 0:248f3186c666 919 OperationMode=AUTOLOOP;
TUATBM 0:248f3186c666 920 //pc.printf("go to manualmade\r\n");
TUATBM 0:248f3186c666 921 }
TUATBM 0:248f3186c666 922 break;
TUATBM 0:248f3186c666 923
TUATBM 0:248f3186c666 924 case MANUALMODE:
TUATBM 0:248f3186c666 925 update_mode = UPD_MANUAL;
TUATBM 0:248f3186c666 926 led1 = 0;
TUATBM 0:248f3186c666 927 led2 = 1;
TUATBM 0:248f3186c666 928 led4 = 1;
TUATBM 0:248f3186c666 929
TUATBM 0:248f3186c666 930 if(!CheckSW_up(7)&&!CheckSW_up(8)){
TUATBM 0:248f3186c666 931 OperationMode=AUTOLANDING;
TUATBM 0:248f3186c666 932 pc.printf("go to autolanding\r\n");
TUATBM 0:248f3186c666 933 }
TUATBM 0:248f3186c666 934 break;
TUATBM 0:248f3186c666 935
TUATBM 0:248f3186c666 936 }
TUATBM 0:248f3186c666 937
TUATBM 0:248f3186c666 938
TUATBM 0:248f3186c666 939
TUATBM 0:248f3186c666 940
TUATBM 0:248f3186c666 941 /*
TUATBM 0:248f3186c666 942 if(CheckSW_up(7)){
TUATBM 0:248f3186c666 943 pc.printf("true\r\n");
TUATBM 0:248f3186c666 944 Auto_loop();
TUATBM 0:248f3186c666 945
TUATBM 0:248f3186c666 946 }else{
TUATBM 0:248f3186c666 947 pc.printf("false\r\n");
TUATBM 0:248f3186c666 948 update_manual();
TUATBM 0:248f3186c666 949 }*/
TUATBM 0:248f3186c666 950 }
TUATBM 0:248f3186c666 951 }