勇輝 関本 / Mbed 2 deprecated Estrela_v10

４項目一応成功

Dependencies:   mbed

Fork of Estrela_v01 by Bot Furukawa

main.cpp@4:ba610b05751d, 2016-08-21 (annotated)

Committer:
motoseki
Date:
Sun Aug 21 01:39:04 2016 +0000
Revision:
4:ba610b05751d
Parent:
3:8c01b4f33389
Child:
5:c53ca479e96c
?????????

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