勇輝 関本 / Mbed 2 deprecated Estrela_v10

４項目一応成功

Dependencies:   mbed

Fork of Estrela_v01 by Bot Furukawa

main.cpp@2:b88f73d7073c, 2016-08-17 (annotated)

Committer:
TUATBM
Date:
Wed Aug 17 12:47:59 2016 +0000
Revision:
2:b88f73d7073c
Parent:
1:fc5988ebfa53
Child:
3:8c01b4f33389
?????????????

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 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++){
TUATBM 0:4013a9855dc8 127 pc.printf("ch%d=%d ", j+1, channels[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;
TUATBM 0:4013a9855dc8 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 0:4013a9855dc8 359 //pc.printf("gx = %f", mx);
TUATBM 0:4013a9855dc8 360 //pc.printf(" gy = %f", my);
TUATBM 0:4013a9855dc8 361 //pc.printf(" gz = %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]);
TUATBM 0:4013a9855dc8 383 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
TUATBM 0:4013a9855dc8 384 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 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
TUATBM 2:b88f73d7073c 390 //pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll);
TUATBM 0:4013a9855dc8 391 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
TUATBM 0:4013a9855dc8 392
TUATBM 0:4013a9855dc8 393 count = t.read_ms();
TUATBM 0:4013a9855dc8 394 sum = 0;
TUATBM 0:4013a9855dc8 395 sumCount = 0;
TUATBM 2:b88f73d7073c 396 //}
TUATBM 0:4013a9855dc8 397
TUATBM 0:4013a9855dc8 398 }
TUATBM 0:4013a9855dc8 399
TUATBM 0:4013a9855dc8 400 //---StabiGyro---
TUATBM 0:4013a9855dc8 401 //水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
TUATBM 0:4013a9855dc8 402 void StabiGyro()
TUATBM 0:4013a9855dc8 403 {
TUATBM 0:4013a9855dc8 404 if(jj<=25){ //旋回し始め
TUATBM 2:b88f73d7073c 405 Auto_RUD = RUD_N - 30 - int((gy+130)*0.75); //roll -> gy
TUATBM 2:b88f73d7073c 406 Auto_ELE = ELE_N + 30 - int((gx-30)*0.8); //pitch -> -gx
TUATBM 0:4013a9855dc8 407 Auto_THR = THR_N + 50;
TUATBM 0:4013a9855dc8 408 //pc.printf("first%d\r\n",jj);
TUATBM 0:4013a9855dc8 409 jj++;
TUATBM 0:4013a9855dc8 410 }else{ //旋回中
TUATBM 2:b88f73d7073c 411 Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
TUATBM 2:b88f73d7073c 412 Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
TUATBM 0:4013a9855dc8 413 Auto_THR = THR_N;
TUATBM 0:4013a9855dc8 414 }
TUATBM 0:4013a9855dc8 415
TUATBM 0:4013a9855dc8 416 //pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする
TUATBM 0:4013a9855dc8 417 if(Auto_RUD > 1790){
TUATBM 0:4013a9855dc8 418 Auto_RUD = 1790;
TUATBM 0:4013a9855dc8 419 }else if(Auto_RUD < 1032){
TUATBM 0:4013a9855dc8 420 Auto_RUD = 1032;
TUATBM 0:4013a9855dc8 421 }
TUATBM 0:4013a9855dc8 422 if(Auto_ELE > 1690){
TUATBM 0:4013a9855dc8 423 Auto_ELE = 1690;
TUATBM 0:4013a9855dc8 424 }else if(Auto_ELE < 1192){
TUATBM 0:4013a9855dc8 425 Auto_ELE = 1192;
TUATBM 0:4013a9855dc8 426 }
TUATBM 0:4013a9855dc8 427
TUATBM 0:4013a9855dc8 428 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
TUATBM 0:4013a9855dc8 429 //Thread::wait(G_MS);
TUATBM 0:4013a9855dc8 430 }
TUATBM 0:4013a9855dc8 431
TUATBM 0:4013a9855dc8 432 //---StabiGyro_Mobius---
TUATBM 0:4013a9855dc8 433 //8の字旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
TUATBM 0:4013a9855dc8 434 void StabiGyro_Mobius()
TUATBM 0:4013a9855dc8 435 {
TUATBM 0:4013a9855dc8 436 if(jj<=25){ //右旋回導入
TUATBM 2:b88f73d7073c 437 Auto_RUD = RUD_N - 30 - int((gy+130)*0.75);
TUATBM 2:b88f73d7073c 438 Auto_ELE = ELE_N + 30 - int((gx-30)*0.8);
TUATBM 0:4013a9855dc8 439 Auto_THR = THR_N + 50;
TUATBM 0:4013a9855dc8 440 //pc.printf("first%d\r\n",jj);
TUATBM 0:4013a9855dc8 441 jj++;
TUATBM 0:4013a9855dc8 442 }else if((25<jj) && (jj<=112)){ //右旋回途中
TUATBM 2:b88f73d7073c 443 Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
TUATBM 2:b88f73d7073c 444 Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
TUATBM 0:4013a9855dc8 445 Auto_THR = THR_N;
TUATBM 0:4013a9855dc8 446 jj++;
TUATBM 0:4013a9855dc8 447 }else if((112<jj) && (jj<=117)){ //旋回遷移1
TUATBM 2:b88f73d7073c 448 Auto_RUD = RUD_N + 160 - int((gy-100)*0.75);
TUATBM 2:b88f73d7073c 449 Auto_ELE = ELE_N + 20 - int((gx+0)*0.6);
TUATBM 0:4013a9855dc8 450 Auto_THR = THR_N - 100;
TUATBM 0:4013a9855dc8 451 //pc.printf("first%d\r\n",jj);
TUATBM 0:4013a9855dc8 452 jj++;
TUATBM 0:4013a9855dc8 453 }else if((117<jj) && (jj<=132)){ //旋回遷移2
TUATBM 2:b88f73d7073c 454 Auto_RUD = RUD_N + 120 - int((gy-100)*0.75);
TUATBM 2:b88f73d7073c 455 Auto_ELE = ELE_N + 65 - int((gx-30)*0.6);
TUATBM 0:4013a9855dc8 456 Auto_THR = THR_N - 20;
TUATBM 0:4013a9855dc8 457 jj++;
TUATBM 0:4013a9855dc8 458 }else{ //左旋回途中
TUATBM 2:b88f73d7073c 459 Auto_RUD = RUD_N + 120 - int((gy-25)*0.75);
TUATBM 2:b88f73d7073c 460 Auto_ELE = ELE_N + 0 - int((gx-30)*0.6);
TUATBM 0:4013a9855dc8 461 Auto_THR = THR_N + 30;
TUATBM 0:4013a9855dc8 462 }
TUATBM 0:4013a9855dc8 463
TUATBM 0:4013a9855dc8 464 if(Auto_RUD > 1790){
TUATBM 0:4013a9855dc8 465 Auto_RUD = 1790;
TUATBM 0:4013a9855dc8 466 }else if(Auto_RUD < 1032){
TUATBM 0:4013a9855dc8 467 Auto_RUD = 1032;
TUATBM 0:4013a9855dc8 468 }
TUATBM 0:4013a9855dc8 469 if(Auto_ELE > 1690){
TUATBM 0:4013a9855dc8 470 Auto_ELE = 1690;
TUATBM 0:4013a9855dc8 471 }else if(Auto_ELE < 1192){
TUATBM 0:4013a9855dc8 472 Auto_ELE = 1192;
TUATBM 0:4013a9855dc8 473 }
TUATBM 0:4013a9855dc8 474
TUATBM 0:4013a9855dc8 475 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
TUATBM 0:4013a9855dc8 476 //Thread::wait(G_MS);
TUATBM 0:4013a9855dc8 477 }
TUATBM 0:4013a9855dc8 478
TUATBM 0:4013a9855dc8 479 //---StabiGyro_Climb---
TUATBM 0:4013a9855dc8 480 //上昇旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
TUATBM 0:4013a9855dc8 481 void StabiGyro_Climb()
TUATBM 0:4013a9855dc8 482 {
TUATBM 0:4013a9855dc8 483 if(jj<=25){ //右旋回導入
TUATBM 2:b88f73d7073c 484 Auto_RUD = RUD_N - 30 - int((gy+130)*0.75);
TUATBM 2:b88f73d7073c 485 Auto_ELE = ELE_N + 30 - int((gx-30)*0.8);
TUATBM 0:4013a9855dc8 486 Auto_THR = THR_N + 50;
TUATBM 0:4013a9855dc8 487 //pc.printf("first%d\r\n",jj);
TUATBM 0:4013a9855dc8 488 jj++;
TUATBM 0:4013a9855dc8 489 }else if((25<jj) && (jj<=224)){ //右旋回途中
TUATBM 2:b88f73d7073c 490 Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
TUATBM 2:b88f73d7073c 491 Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
TUATBM 0:4013a9855dc8 492 Auto_THR = THR_N;
TUATBM 0:4013a9855dc8 493 jj++;
TUATBM 0:4013a9855dc8 494 /*}else if((224<jj) && (jj<=234)){ //上昇遷移
TUATBM 0:4013a9855dc8 495 Auto_RUD = RUD_N + 160 - int((g_HpfGyro[0]+25)*0.75);
TUATBM 0:4013a9855dc8 496 Auto_ELE = ELE_N + 20 + int((g_HpfGyro[1]-30)*0.8);
TUATBM 0:4013a9855dc8 497 Auto_THR = THR_N + 100;
TUATBM 0:4013a9855dc8 498 //pc.printf("first%d\r\n",jj);
TUATBM 0:4013a9855dc8 499 jj++;*/
TUATBM 0:4013a9855dc8 500 }else if((224<jj) && (jj<=314)){ //上昇中
TUATBM 2:b88f73d7073c 501 Auto_RUD = RUD_N - 10 - int((gy+25)*0.75);
TUATBM 2:b88f73d7073c 502 Auto_ELE = ELE_N - 35 - int((gx-30)*0.8);
TUATBM 0:4013a9855dc8 503 Auto_THR = THR_N + 220;
TUATBM 0:4013a9855dc8 504 jj++;
TUATBM 0:4013a9855dc8 505 }else{ //水平旋回
TUATBM 2:b88f73d7073c 506 Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
TUATBM 2:b88f73d7073c 507 Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
TUATBM 0:4013a9855dc8 508 Auto_THR = THR_N;
TUATBM 0:4013a9855dc8 509 }
TUATBM 0:4013a9855dc8 510
TUATBM 0:4013a9855dc8 511 if(Auto_RUD > 1790){
TUATBM 0:4013a9855dc8 512 Auto_RUD = 1790;
TUATBM 0:4013a9855dc8 513 }else if(Auto_RUD < 1032){
TUATBM 0:4013a9855dc8 514 Auto_RUD = 1032;
TUATBM 0:4013a9855dc8 515 }
TUATBM 0:4013a9855dc8 516 if(Auto_ELE > 1690){
TUATBM 0:4013a9855dc8 517 Auto_ELE = 1690;
TUATBM 0:4013a9855dc8 518 }else if(Auto_ELE < 1192){
TUATBM 0:4013a9855dc8 519 Auto_ELE = 1192;
TUATBM 0:4013a9855dc8 520 }
TUATBM 0:4013a9855dc8 521
TUATBM 0:4013a9855dc8 522 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
TUATBM 0:4013a9855dc8 523 //Thread::wait(G_MS);
TUATBM 0:4013a9855dc8 524 }
TUATBM 0:4013a9855dc8 525
TUATBM 0:4013a9855dc8 526 //---StabiGyro_Glide---
TUATBM 0:4013a9855dc8 527 //自動滑空のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
TUATBM 0:4013a9855dc8 528 void StabiGyro_Glide()
TUATBM 0:4013a9855dc8 529 {
TUATBM 0:4013a9855dc8 530 //if(jj<=25){
TUATBM 0:4013a9855dc8 531 // Auto_RUD = RUD_N - 30 - int((g_HpfGyro[0]+130)*0.75);
TUATBM 0:4013a9855dc8 532 // Auto_ELE = ELE_N + 30+ int((g_HpfGyro[1]-30)*0.8);
TUATBM 0:4013a9855dc8 533 // Auto_THR = THR_N + 50;
TUATBM 0:4013a9855dc8 534 //pc.printf("first%d\r\n",jj);
TUATBM 0:4013a9855dc8 535 // jj++;
TUATBM 0:4013a9855dc8 536 //}else{
TUATBM 2:b88f73d7073c 537 Auto_RUD = RUD_N - 10 - int((gy+10)*0.75);
TUATBM 2:b88f73d7073c 538 Auto_ELE = ELE_N - 10 - int((gx-15)*0.8);
TUATBM 0:4013a9855dc8 539 Auto_THR = 1000; //スロットルoff
TUATBM 0:4013a9855dc8 540 //}
TUATBM 0:4013a9855dc8 541
TUATBM 0:4013a9855dc8 542 if(Auto_RUD > 1790){
TUATBM 0:4013a9855dc8 543 Auto_RUD = 1790;
TUATBM 0:4013a9855dc8 544 }else if(Auto_RUD < 1032){
TUATBM 0:4013a9855dc8 545 Auto_RUD = 1032;
TUATBM 0:4013a9855dc8 546 }
TUATBM 0:4013a9855dc8 547 if(Auto_ELE > 1690){
TUATBM 0:4013a9855dc8 548 Auto_ELE = 1690;
TUATBM 0:4013a9855dc8 549 }else if(Auto_ELE < 1192){
TUATBM 0:4013a9855dc8 550 Auto_ELE = 1192;
TUATBM 0:4013a9855dc8 551 }
TUATBM 0:4013a9855dc8 552
TUATBM 0:4013a9855dc8 553 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
TUATBM 0:4013a9855dc8 554 //Thread::wait(G_MS);
TUATBM 0:4013a9855dc8 555 }
TUATBM 0:4013a9855dc8 556
TUATBM 0:4013a9855dc8 557 void Auto_Loop()
TUATBM 0:4013a9855dc8 558 {
TUATBM 0:4013a9855dc8 559 //while(true){
TUATBM 0:4013a9855dc8 560 sensing(); //センサーの値を読み込む(認知)
TUATBM 0:4013a9855dc8 561 StabiGyro(); //水平旋回のためのラダー、エレベーターのpwmを計算(判断)
TUATBM 0:4013a9855dc8 562 update_mode = UPD_AUTO; //オートモード
TUATBM 0:4013a9855dc8 563
TUATBM 0:4013a9855dc8 564 //StabiAccel();
TUATBM 0:4013a9855dc8 565 //Auto_ELE+=145;
TUATBM 0:4013a9855dc8 566 //Auto_RUD-=80;
TUATBM 0:4013a9855dc8 567 //Servos.Auto2Servo();
TUATBM 0:4013a9855dc8 568 //Thread::wait(50);
TUATBM 0:4013a9855dc8 569 wait_ms(50);
TUATBM 0:4013a9855dc8 570 //}
TUATBM 0:4013a9855dc8 571 }
TUATBM 0:4013a9855dc8 572
TUATBM 0:4013a9855dc8 573 void Auto_Mobius()
TUATBM 0:4013a9855dc8 574 {
TUATBM 0:4013a9855dc8 575 sensing();
TUATBM 0:4013a9855dc8 576 StabiGyro_Mobius();
TUATBM 0:4013a9855dc8 577 update_mode = UPD_AUTO;
TUATBM 0:4013a9855dc8 578 wait_ms(50);
TUATBM 0:4013a9855dc8 579 }
TUATBM 0:4013a9855dc8 580
TUATBM 0:4013a9855dc8 581 void Auto_Climb()
TUATBM 0:4013a9855dc8 582 {
TUATBM 0:4013a9855dc8 583 sensing();
TUATBM 0:4013a9855dc8 584 StabiGyro_Climb();
TUATBM 0:4013a9855dc8 585 update_mode = UPD_AUTO;
TUATBM 0:4013a9855dc8 586 wait_ms(50);
TUATBM 0:4013a9855dc8 587 }
TUATBM 0:4013a9855dc8 588
TUATBM 0:4013a9855dc8 589 void Auto_Glide()
TUATBM 0:4013a9855dc8 590 {
TUATBM 0:4013a9855dc8 591 sensing();
TUATBM 0:4013a9855dc8 592 StabiGyro_Glide();
TUATBM 0:4013a9855dc8 593 update_mode = UPD_AUTO;
TUATBM 0:4013a9855dc8 594 wait_ms(50);
TUATBM 0:4013a9855dc8 595 }
TUATBM 0:4013a9855dc8 596
TUATBM 0:4013a9855dc8 597 void Auto_Landing()
TUATBM 0:4013a9855dc8 598 {
TUATBM 0:4013a9855dc8 599 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 600 }
TUATBM 0:4013a9855dc8 601
TUATBM 0:4013a9855dc8 602 int main()
TUATBM 0:4013a9855dc8 603 {
TUATBM 0:4013a9855dc8 604 //SBus用バッファ初期化
TUATBM 0:4013a9855dc8 605 int i;
TUATBM 0:4013a9855dc8 606 for (i=0; i<25; i++) {buf_sbus[i] = 0;}
TUATBM 0:4013a9855dc8 607
TUATBM 0:4013a9855dc8 608 //pc.baud(115200); //パソコン通信用シリアルのボードレート
TUATBM 0:4013a9855dc8 609
TUATBM 0:4013a9855dc8 610 //frq.start();
TUATBM 0:4013a9855dc8 611 init_Servo();
TUATBM 0:4013a9855dc8 612 init_sbus();
TUATBM 0:4013a9855dc8 613 disp_clock();
TUATBM 0:4013a9855dc8 614
TUATBM 0:4013a9855dc8 615 //---9軸センサー(MPU9250)初期化---
TUATBM 0:4013a9855dc8 616
TUATBM 0:4013a9855dc8 617 //Set up I2C
TUATBM 0:4013a9855dc8 618 i2c.frequency(400000); // use fast (400 kHz) I2C
TUATBM 0:4013a9855dc8 619 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
TUATBM 0:4013a9855dc8 620 t.start();
TUATBM 0:4013a9855dc8 621
TUATBM 0:4013a9855dc8 622 // Read the WHO_AM_I register, this is a good test of communication
TUATBM 0:4013a9855dc8 623 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
TUATBM 0:4013a9855dc8 624 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
TUATBM 0:4013a9855dc8 625
TUATBM 0:4013a9855dc8 626 if (whoami == 0x71){ // WHO_AM_I should always be 0x68
TUATBM 0:4013a9855dc8 627 pc.printf("MPU9250 is online...\n\r");
TUATBM 1:fc5988ebfa53 628 //wait(1);
TUATBM 0:4013a9855dc8 629
TUATBM 0:4013a9855dc8 630 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
TUATBM 0:4013a9855dc8 631 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
TUATBM 0:4013a9855dc8 632 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
TUATBM 0:4013a9855dc8 633 //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
TUATBM 0:4013a9855dc8 634 //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
TUATBM 0:4013a9855dc8 635 //pc.printf("x accel bias = %f\n\r", accelBias[0]);
TUATBM 0:4013a9855dc8 636 //pc.printf("y accel bias = %f\n\r", accelBias[1]);
TUATBM 0:4013a9855dc8 637 //pc.printf("z accel bias = %f\n\r", accelBias[2]);
TUATBM 1:fc5988ebfa53 638 wait(1);
TUATBM 0:4013a9855dc8 639
TUATBM 0:4013a9855dc8 640 mpu9250.initMPU9250();
TUATBM 0:4013a9855dc8 641 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
TUATBM 0:4013a9855dc8 642 mpu9250.initAK8963(magCalibration);
TUATBM 0:4013a9855dc8 643 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
TUATBM 0:4013a9855dc8 644 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
TUATBM 0:4013a9855dc8 645 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
TUATBM 0:4013a9855dc8 646 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
TUATBM 0:4013a9855dc8 647 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
TUATBM 0:4013a9855dc8 648 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
TUATBM 0:4013a9855dc8 649 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
TUATBM 0:4013a9855dc8 650 wait(2);
TUATBM 0:4013a9855dc8 651
TUATBM 0:4013a9855dc8 652 }else{
TUATBM 0:4013a9855dc8 653 pc.printf("Could not connect to MPU9250: \n\r");
TUATBM 0:4013a9855dc8 654 pc.printf("%#x \n", whoami);
TUATBM 0:4013a9855dc8 655 while(1) ; // Loop forever if communication doesn't happen
TUATBM 0:4013a9855dc8 656 }
TUATBM 0:4013a9855dc8 657
TUATBM 0:4013a9855dc8 658 mpu9250.getAres(); // Get accelerometer sensitivity
TUATBM 0:4013a9855dc8 659 mpu9250.getGres(); // Get gyro sensitivity
TUATBM 0:4013a9855dc8 660 mpu9250.getMres(); // Get magnetometer sensitivity
TUATBM 0:4013a9855dc8 661 //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
TUATBM 0:4013a9855dc8 662 //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
TUATBM 0:4013a9855dc8 663 //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
TUATBM 0:4013a9855dc8 664 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
TUATBM 0:4013a9855dc8 665 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
TUATBM 0:4013a9855dc8 666 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
TUATBM 0:4013a9855dc8 667
TUATBM 0:4013a9855dc8 668 pc.printf("mpu9250 initialized\r\n");
TUATBM 0:4013a9855dc8 669
TUATBM 0:4013a9855dc8 670 //---9軸センサー(MPU9250)初期化終わり---
TUATBM 0:4013a9855dc8 671
TUATBM 0:4013a9855dc8 672 led1 = 0;
TUATBM 0:4013a9855dc8 673 led2 = 0;
TUATBM 1:fc5988ebfa53 674 led3 = 0;
TUATBM 1:fc5988ebfa53 675 led4 = 0;
TUATBM 0:4013a9855dc8 676
TUATBM 0:4013a9855dc8 677 pc.printf("All initialized\r\n");
TUATBM 0:4013a9855dc8 678
TUATBM 0:4013a9855dc8 679
TUATBM 0:4013a9855dc8 680 while (true) { //無限ループ
TUATBM 0:4013a9855dc8 681 //for(i=0;i<8;i++){
TUATBM 0:4013a9855dc8 682 //pc.printf("%x ", buf_sbus[i]);
TUATBM 0:4013a9855dc8 683 //pc.printf("ch%d=%d ", i+1, channels[i]);
TUATBM 0:4013a9855dc8 684 //}
TUATBM 0:4013a9855dc8 685 //pc.printf("\n");
TUATBM 2:b88f73d7073c 686 //wait_ms(500);
TUATBM 0:4013a9855dc8 687
TUATBM 0:4013a9855dc8 688
TUATBM 0:4013a9855dc8 689 switch(OperationMode){ //変数OperationModeの値で場合分け
TUATBM 0:4013a9855dc8 690 case GROUNDCHECK: //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行)
TUATBM 0:4013a9855dc8 691 if(!CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7,8が両方offなら
TUATBM 0:4013a9855dc8 692 OperationMode=AUTOLOOP; //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入
TUATBM 0:4013a9855dc8 693 pc.printf("go to autoloop\r\n");
TUATBM 0:4013a9855dc8 694 }
TUATBM 0:4013a9855dc8 695 //変数update_modeはSBus(=プロポ＝操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数
TUATBM 0:4013a9855dc8 696 update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す)
TUATBM 0:4013a9855dc8 697 pc.printf("groundcheck mode\r\n");
TUATBM 0:4013a9855dc8 698
TUATBM 0:4013a9855dc8 699 break;
TUATBM 0:4013a9855dc8 700 case AUTOLOOP: //水平旋回モード
TUATBM 0:4013a9855dc8 701 if(CheckSW_up(7)){ //チャンネル7がonなら
TUATBM 0:4013a9855dc8 702 Auto_Loop(); //関数Auto_Loop実行
TUATBM 0:4013a9855dc8 703 led1 = 0;
TUATBM 0:4013a9855dc8 704 led2 = 1;
TUATBM 0:4013a9855dc8 705 pc.printf("Auto Loop Now\r\n");
TUATBM 0:4013a9855dc8 706 //autoloop.set_priority(osPriorityAboveNormal);
TUATBM 0:4013a9855dc8 707 /*
TUATBM 0:4013a9855dc8 708 while(true){
TUATBM 0:4013a9855dc8 709 if(!CheckSW_up(7)){
TUATBM 0:4013a9855dc8 710 autoloop.terminate();
TUATBM 0:4013a9855dc8 711 break;
TUATBM 0:4013a9855dc8 712 }
TUATBM 0:4013a9855dc8 713 }
TUATBM 0:4013a9855dc8 714 */
TUATBM 0:4013a9855dc8 715 }else{
TUATBM 0:4013a9855dc8 716 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 717 pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 718 led1 = 1;
TUATBM 0:4013a9855dc8 719 led2 = 0;
TUATBM 0:4013a9855dc8 720 }
TUATBM 0:4013a9855dc8 721
TUATBM 0:4013a9855dc8 722 if(!CheckSW_up(7)&&CheckSW_up(8)){ //チャンネル7がoff、8がonなら
TUATBM 0:4013a9855dc8 723 OperationMode=AUTOMOBIUS;
TUATBM 0:4013a9855dc8 724 pc.printf("go to auto8\r\n");
TUATBM 0:4013a9855dc8 725 }
TUATBM 0:4013a9855dc8 726 break;
TUATBM 0:4013a9855dc8 727 case AUTOMOBIUS: //8の字旋回モード
TUATBM 0:4013a9855dc8 728 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 729 Auto_Mobius();
TUATBM 0:4013a9855dc8 730 led1 = 0;
TUATBM 0:4013a9855dc8 731 led2 = 1;
TUATBM 0:4013a9855dc8 732 pc.printf("Auto Mobius Now\r\n");
TUATBM 0:4013a9855dc8 733 }else{
TUATBM 0:4013a9855dc8 734 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 735 pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 736 led1 = 1;
TUATBM 0:4013a9855dc8 737 led2 = 1;
TUATBM 0:4013a9855dc8 738 }
TUATBM 0:4013a9855dc8 739
TUATBM 0:4013a9855dc8 740 if(!CheckSW_up(7)&&!CheckSW_up(8)){
TUATBM 0:4013a9855dc8 741 OperationMode=AUTOCLIMB;
TUATBM 0:4013a9855dc8 742 pc.printf("go to autoclimb\r\n");
TUATBM 0:4013a9855dc8 743 }
TUATBM 0:4013a9855dc8 744 break;
TUATBM 0:4013a9855dc8 745 case AUTOCLIMB: //上昇旋回モード
TUATBM 0:4013a9855dc8 746 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 747 Auto_Climb();
TUATBM 0:4013a9855dc8 748 led1 = 0;
TUATBM 0:4013a9855dc8 749 led2 = 1;
TUATBM 0:4013a9855dc8 750 pc.printf("Auto Climb 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 = 0;
TUATBM 0:4013a9855dc8 756 }
TUATBM 0:4013a9855dc8 757
TUATBM 0:4013a9855dc8 758 if(!CheckSW_up(7)&&CheckSW_up(8)){
TUATBM 0:4013a9855dc8 759 OperationMode=AUTOGLIDE;
TUATBM 0:4013a9855dc8 760 pc.printf("go to manual\r\n");
TUATBM 0:4013a9855dc8 761 }
TUATBM 0:4013a9855dc8 762 break;
TUATBM 0:4013a9855dc8 763
TUATBM 0:4013a9855dc8 764 case AUTOGLIDE: //自動滑空モード
TUATBM 0:4013a9855dc8 765 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 766 Auto_Glide();
TUATBM 0:4013a9855dc8 767 led1 = 0;
TUATBM 0:4013a9855dc8 768 led2 = 1;
TUATBM 0:4013a9855dc8 769 pc.printf("Auto Glide Now\r\n");
TUATBM 0:4013a9855dc8 770 }else{
TUATBM 0:4013a9855dc8 771 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 772 pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 773 led1 = 1;
TUATBM 0:4013a9855dc8 774 led2 = 1;
TUATBM 0:4013a9855dc8 775 }
TUATBM 0:4013a9855dc8 776
TUATBM 0:4013a9855dc8 777 if(!CheckSW_up(7)&&!CheckSW_up(8)){
TUATBM 0:4013a9855dc8 778 OperationMode=AUTOLOOP;
TUATBM 0:4013a9855dc8 779 pc.printf("go to autoloop\r\n");
TUATBM 0:4013a9855dc8 780 }
TUATBM 0:4013a9855dc8 781 break;
TUATBM 0:4013a9855dc8 782 /*
TUATBM 0:4013a9855dc8 783 case AUTOLANDING:
TUATBM 0:4013a9855dc8 784 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 785 Auto_Landing();
TUATBM 0:4013a9855dc8 786 led1 = 0;
TUATBM 0:4013a9855dc8 787 led2 = 1;
TUATBM 0:4013a9855dc8 788 pc.printf("Auto Landing Now\r\n");
TUATBM 0:4013a9855dc8 789 }else{
TUATBM 0:4013a9855dc8 790 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 791 pc.printf("manual Now\r\n");
TUATBM 0:4013a9855dc8 792 led1 = 1;
TUATBM 0:4013a9855dc8 793 led2 = 0;
TUATBM 0:4013a9855dc8 794 }
TUATBM 0:4013a9855dc8 795
TUATBM 0:4013a9855dc8 796 if(!CheckSW_up(7)&&CheckSW_up(8)){
TUATBM 0:4013a9855dc8 797 OperationMode=MANUALMODE;
TUATBM 0:4013a9855dc8 798 pc.printf("go to manualmade\r\n");
TUATBM 0:4013a9855dc8 799 }
TUATBM 0:4013a9855dc8 800 break;
TUATBM 0:4013a9855dc8 801
TUATBM 0:4013a9855dc8 802 case MANUALMODE:
TUATBM 0:4013a9855dc8 803 update_mode = UPD_MANUAL;
TUATBM 0:4013a9855dc8 804 led1 = 1;
TUATBM 0:4013a9855dc8 805 led2 = 1;
TUATBM 0:4013a9855dc8 806
TUATBM 0:4013a9855dc8 807 if(!CheckSW_up(7)&&!CheckSW_up(8)){
TUATBM 0:4013a9855dc8 808 OperationMode=AUTOLOOP;
TUATBM 0:4013a9855dc8 809 pc.printf("go to autoloop\r\n");
TUATBM 0:4013a9855dc8 810 }
TUATBM 0:4013a9855dc8 811 break;
TUATBM 0:4013a9855dc8 812 */
TUATBM 0:4013a9855dc8 813 }
TUATBM 0:4013a9855dc8 814
TUATBM 0:4013a9855dc8 815
TUATBM 0:4013a9855dc8 816
TUATBM 0:4013a9855dc8 817
TUATBM 0:4013a9855dc8 818 /*
TUATBM 0:4013a9855dc8 819 if(CheckSW_up(7)){
TUATBM 0:4013a9855dc8 820 pc.printf("true\r\n");
TUATBM 0:4013a9855dc8 821 Auto_loop();
TUATBM 0:4013a9855dc8 822
TUATBM 0:4013a9855dc8 823 }else{
TUATBM 0:4013a9855dc8 824 pc.printf("false\r\n");
TUATBM 0:4013a9855dc8 825 update_manual();
TUATBM 0:4013a9855dc8 826 }*/
TUATBM 0:4013a9855dc8 827 }
TUATBM 0:4013a9855dc8 828 }