勇輝 関本 / Mbed 2 deprecated Estrela_v10

４項目一応成功

Dependencies:   mbed

Fork of Estrela_v01 by Bot Furukawa

main.cpp@0:4013a9855dc8, 2016-08-17 (annotated)

Committer:
TUATBM
Date:
Wed Aug 17 04:06:17 2016 +0000
Revision:
0:4013a9855dc8
Child:
1:fc5988ebfa53
8/17

Who changed what in which revision?

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