Atsumi Toda / Mbed OS UAV_Logger1

UAVの姿勢推定に使用するプログラム。

Dependencies:   MPU6050_alter

main.cpp@4:21a356ae0747, 2019-08-19 (annotated)

Committer:
Joeatsumi
Date:
Mon Aug 19 02:50:13 2019 +0000
Revision:
4:21a356ae0747
Parent:
3:3fa7882a5fd0 
test program for auto pilot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 4:21a356ae0747 1 /*
Joeatsumi 4:21a356ae0747 2 This system is vertion 2.0. Thsi program enable us to read signals from receiver ,measure balance of circit, and actuate servos.
Joeatsumi 4:21a356ae0747 3 */
Joeatsumi 4:21a356ae0747 4
hudakz 0:87642278ede6 5 #include "mbed.h"
Joeatsumi 4:21a356ae0747 6 #include "string.h"
Joeatsumi 4:21a356ae0747 7 #include "MPU6050.h"
Joeatsumi 4:21a356ae0747 8
hudakz 0:87642278ede6 9 #include <stdio.h>
hudakz 0:87642278ede6 10 #include <errno.h>
Joeatsumi 2:e6496a794bde 11
hudakz 0:87642278ede6 12 #include "SDBlockDevice.h"
hudakz 0:87642278ede6 13 #include "FATFileSystem.h"
hudakz 0:87642278ede6 14
Joeatsumi 4:21a356ae0747 15 #include "myConstants.h"
Joeatsumi 4:21a356ae0747 16
Joeatsumi 4:21a356ae0747 17 #define READBUFFERSIZE 70
Joeatsumi 4:21a356ae0747 18 #define DELIMITER ","
Joeatsumi 2:e6496a794bde 19 #define M_PI 3.141592
Joeatsumi 2:e6496a794bde 20 #define PI2 (2*M_PI)
Joeatsumi 2:e6496a794bde 21
Joeatsumi 2:e6496a794bde 22 /*ジャイロセンサの補正値（引く）*/
Joeatsumi 2:e6496a794bde 23 #define GYRO_FIX_X 290.5498
Joeatsumi 2:e6496a794bde 24 #define GYRO_FIX_Y 99.29363
Joeatsumi 2:e6496a794bde 25 #define GYRO_FIX_Z 61.41011
hudakz 0:87642278ede6 26
Joeatsumi 4:21a356ae0747 27 /*---------ピン指定-------------------*/
Joeatsumi 4:21a356ae0747 28 DigitalOut myled(LED1);
Joeatsumi 4:21a356ae0747 29
Joeatsumi 4:21a356ae0747 30 DigitalIn switch_off(p30);//sdカードへの書き込みを終了させる
Joeatsumi 4:21a356ae0747 31
Joeatsumi 4:21a356ae0747 32 InterruptIn button(p5);//GPSモジュールからの1pps信号を読み取る（立ち下がり割り込み）
hudakz 0:87642278ede6 33
Joeatsumi 4:21a356ae0747 34 InterruptIn input_from_Aileron_1(p11);
Joeatsumi 4:21a356ae0747 35 InterruptIn input_from_elevator(p12);//
Joeatsumi 4:21a356ae0747 36 InterruptIn input_from_throttle(p13);//
Joeatsumi 4:21a356ae0747 37 InterruptIn input_from_rudder(p14);//
Joeatsumi 4:21a356ae0747 38 InterruptIn input_from_gear(p15);//
Joeatsumi 4:21a356ae0747 39 InterruptIn input_from_Aileron_2(p16);//
Joeatsumi 4:21a356ae0747 40 InterruptIn input_from_pitch(p17);//
Joeatsumi 4:21a356ae0747 41 InterruptIn input_from_AUX5(p18);//
Joeatsumi 4:21a356ae0747 42
Joeatsumi 4:21a356ae0747 43
Joeatsumi 4:21a356ae0747 44 MPU6050 mpu(p28, p27);//sda scl
Joeatsumi 4:21a356ae0747 45
Joeatsumi 4:21a356ae0747 46 Serial pc(USBTX, USBRX); // tx, rx
Joeatsumi 4:21a356ae0747 47 Serial gps(p9,p10);// gpsとの接続 tx, rx
Joeatsumi 4:21a356ae0747 48 SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs
Joeatsumi 4:21a356ae0747 49
Joeatsumi 3:3fa7882a5fd0 50 //declare PWM pins here
Joeatsumi 3:3fa7882a5fd0 51 PwmOut ESC (p21);
Joeatsumi 3:3fa7882a5fd0 52 PwmOut Elevator_servo(p22);
Joeatsumi 3:3fa7882a5fd0 53 PwmOut Rudder_servo(p23);
Joeatsumi 4:21a356ae0747 54 PwmOut Aileron_R_servo(p25);
Joeatsumi 4:21a356ae0747 55 PwmOut Aileron_L_servo(p26);
Joeatsumi 4:21a356ae0747 56
Joeatsumi 4:21a356ae0747 57 /*-----------------------------------*/
Joeatsumi 4:21a356ae0747 58
Joeatsumi 4:21a356ae0747 59 /*-----ファイルポインタ-----*/
Joeatsumi 4:21a356ae0747 60 // File system declaration
Joeatsumi 4:21a356ae0747 61 FATFileSystem fileSystem("fs");
Joeatsumi 4:21a356ae0747 62 /*-----------------------*/
Joeatsumi 4:21a356ae0747 63
Joeatsumi 4:21a356ae0747 64 /*------------------------GPSの変数------------------------*/
Joeatsumi 4:21a356ae0747 65 int GPS_flag_update=0;
Joeatsumi 4:21a356ae0747 66 int GPS_interruptIn=0;
Joeatsumi 4:21a356ae0747 67
Joeatsumi 4:21a356ae0747 68 float longtude_gps,latitude_gps,azimuth_gps,speed_gps;
Joeatsumi 4:21a356ae0747 69
Joeatsumi 4:21a356ae0747 70 //char NMEA_sentense[READBUFFERSIZE]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
Joeatsumi 4:21a356ae0747 71 char NMEA_sentense[READBUFFERSIZE];
Joeatsumi 4:21a356ae0747 72
Joeatsumi 4:21a356ae0747 73 /*--------------------------------------------------------*/
Joeatsumi 3:3fa7882a5fd0 74
Joeatsumi 3:3fa7882a5fd0 75
Joeatsumi 4:21a356ae0747 76 /*--------------------------角度とセンサ--------------------------*/
Joeatsumi 4:21a356ae0747 77 float sensor_array[6];
Joeatsumi 4:21a356ae0747 78 float Acc_Pitch_Roll[2];
Joeatsumi 4:21a356ae0747 79 float Gyro_Pitch_Roll[2];//pitch and roll
Joeatsumi 4:21a356ae0747 80 float omega_z;
Joeatsumi 4:21a356ae0747 81 /*--------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 82
Joeatsumi 3:3fa7882a5fd0 83 /*----------PID コントロール-----------------*/
Joeatsumi 3:3fa7882a5fd0 84 #define Gain_Kp 0.8377
Joeatsumi 3:3fa7882a5fd0 85 #define Gain_Ki 0.4450
Joeatsumi 3:3fa7882a5fd0 86 #define Gain_Kd 0.3890
Joeatsumi 3:3fa7882a5fd0 87
Joeatsumi 3:3fa7882a5fd0 88 double Prop_p,Int_p,Dif_p;
Joeatsumi 3:3fa7882a5fd0 89 double Pre_Prop_p;
Joeatsumi 3:3fa7882a5fd0 90 /*-----------------------------------------*/
Joeatsumi 3:3fa7882a5fd0 91
Joeatsumi 3:3fa7882a5fd0 92 /*-----------サーボコントロール----------------*/
Joeatsumi 3:3fa7882a5fd0 93 //define period of servo here
Joeatsumi 3:3fa7882a5fd0 94 #define SERVO_PERIOD 0.020 // servo requires a 20ms period
Joeatsumi 3:3fa7882a5fd0 95 #define NUTRAL 0.0015
Joeatsumi 3:3fa7882a5fd0 96 #define UPPER_DUTY 0.0020
Joeatsumi 3:3fa7882a5fd0 97 #define LOWER_DUTY 0.0010
Joeatsumi 3:3fa7882a5fd0 98
Joeatsumi 4:21a356ae0747 99 #define THRESHOLD 0.0018
Joeatsumi 4:21a356ae0747 100
Joeatsumi 3:3fa7882a5fd0 101 double pitch_duty,roll_duty,yaw_duty;
Joeatsumi 3:3fa7882a5fd0 102
Joeatsumi 4:21a356ae0747 103 double first_period,second_period,third_period,fourth_period;
Joeatsumi 4:21a356ae0747 104 double fifth_period,sixth_period,seventh_period,eighth_period;
Joeatsumi 4:21a356ae0747 105
Joeatsumi 4:21a356ae0747 106 double first_period_old=0.0,second_period_old=0.0,third_period_old=0.0,fourth_period_old=0.0;
Joeatsumi 4:21a356ae0747 107 double fifth_period_old=0.0,sixth_period_old=0.0,seventh_period_old=0.0,eighth_period_old=0.0;
Joeatsumi 4:21a356ae0747 108
Joeatsumi 4:21a356ae0747 109 double throttle_duty,elevator_duty,rudder_duty,switch_duty,aileron_duty;
Joeatsumi 4:21a356ae0747 110
Joeatsumi 3:3fa7882a5fd0 111 /*------------------------------------------*/
Joeatsumi 3:3fa7882a5fd0 112
Joeatsumi 4:21a356ae0747 113 /*-----タイマー---------*/
Joeatsumi 4:21a356ae0747 114 Timer passed_time;//経過時間の計算
Joeatsumi 4:21a356ae0747 115 Timer ch_time;//
Joeatsumi 2:e6496a794bde 116
Joeatsumi 4:21a356ae0747 117 float Time_new,Time_old=0.0;
Joeatsumi 4:21a356ae0747 118 float Servo_command=0.002;
Joeatsumi 4:21a356ae0747 119 /*---------------------*/
Joeatsumi 4:21a356ae0747 120 /*---------PWM割り込みフラグ-------*/
Joeatsumi 4:21a356ae0747 121 int PWM_flag_update=0;
Joeatsumi 4:21a356ae0747 122 /*-------------------------------*/
Joeatsumi 2:e6496a794bde 123
Joeatsumi 4:21a356ae0747 124 /*----------プロトタイプ宣言----------*/
Joeatsumi 4:21a356ae0747 125 void gps_rx();
Joeatsumi 4:21a356ae0747 126 void gps_read();
Joeatsumi 2:e6496a794bde 127
Joeatsumi 4:21a356ae0747 128 double Degree_to_PWM_duty(double degree);
Joeatsumi 4:21a356ae0747 129 double PID_duty_pitch(double target,double vol,float dt);
Joeatsumi 4:21a356ae0747 130 double PID_duty_rudder(double target,double vol,float dt,float yaw_rate);
Joeatsumi 4:21a356ae0747 131 double PID_duty_roll(double target,double vol,float dt);
Joeatsumi 2:e6496a794bde 132
Joeatsumi 4:21a356ae0747 133 void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]);
Joeatsumi 4:21a356ae0747 134 void scan_update(float box_sensor[6]);
Joeatsumi 4:21a356ae0747 135 void sensor_update();
Joeatsumi 2:e6496a794bde 136
Joeatsumi 4:21a356ae0747 137 //gpsの1pps信号の割り込み
Joeatsumi 4:21a356ae0747 138 void flip();
Joeatsumi 2:e6496a794bde 139
Joeatsumi 2:e6496a794bde 140
Joeatsumi 4:21a356ae0747 141 void Initialize_ESC();
Joeatsumi 4:21a356ae0747 142 void Activate_ESC();
Joeatsumi 2:e6496a794bde 143
Joeatsumi 4:21a356ae0747 144 void Input_Aileron_1();
Joeatsumi 4:21a356ae0747 145 void Input_elevator();
Joeatsumi 4:21a356ae0747 146 void Input_throttle();
Joeatsumi 4:21a356ae0747 147 void Input_rudder();
Joeatsumi 4:21a356ae0747 148 void Input_gear();
Joeatsumi 4:21a356ae0747 149 void Input_Aileron_2();
Joeatsumi 4:21a356ae0747 150 void Input_pitch();
Joeatsumi 4:21a356ae0747 151 void Input_AUX5();
Joeatsumi 2:e6496a794bde 152
Joeatsumi 4:21a356ae0747 153 void Input_Aileron_1_fall();
Joeatsumi 4:21a356ae0747 154 void Input_elevator_fall();
Joeatsumi 4:21a356ae0747 155 void Input_throttle_fall();
Joeatsumi 4:21a356ae0747 156 void Input_rudder_fall();
Joeatsumi 4:21a356ae0747 157 void Input_gear_fall();
Joeatsumi 4:21a356ae0747 158 void Input_Aileron_2_fall();
Joeatsumi 4:21a356ae0747 159 void Input_pitch_fall();
Joeatsumi 4:21a356ae0747 160 void Input_AUX5_fall();
Joeatsumi 4:21a356ae0747 161 /*---------------------------------*/
Joeatsumi 3:3fa7882a5fd0 162
Joeatsumi 3:3fa7882a5fd0 163
Joeatsumi 4:21a356ae0747 164 int main() {
Joeatsumi 3:3fa7882a5fd0 165
Joeatsumi 3:3fa7882a5fd0 166 wait(3);
Joeatsumi 3:3fa7882a5fd0 167
Joeatsumi 3:3fa7882a5fd0 168
Joeatsumi 4:21a356ae0747 169 /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/
Joeatsumi 3:3fa7882a5fd0 170
Joeatsumi 3:3fa7882a5fd0 171
Joeatsumi 3:3fa7882a5fd0 172
Joeatsumi 4:21a356ae0747 173 pc.printf("--- Mbed OS filesystem example ---\n");
hudakz 0:87642278ede6 174
hudakz 0:87642278ede6 175 // Try to mount the filesystem
Joeatsumi 2:e6496a794bde 176 pc.printf("Mounting the filesystem... ");
hudakz 0:87642278ede6 177 fflush(stdout);
hudakz 0:87642278ede6 178
hudakz 0:87642278ede6 179 int err = fileSystem.mount(&blockDevice);
Joeatsumi 2:e6496a794bde 180 pc.printf("%s\n", (err ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 181 if (err) {
hudakz 0:87642278ede6 182 // Reformat if we can't mount the filesystem
hudakz 0:87642278ede6 183 // this should only happen on the first boot
Joeatsumi 2:e6496a794bde 184 pc.printf("No filesystem found, formatting... ");
hudakz 0:87642278ede6 185 fflush(stdout);
hudakz 0:87642278ede6 186 err = fileSystem.reformat(&blockDevice);
Joeatsumi 2:e6496a794bde 187 pc.printf("%s\n", (err ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 188 if (err) {
hudakz 0:87642278ede6 189 error("error: %s (%d)\n", strerror(-err), err);
hudakz 0:87642278ede6 190 }
hudakz 0:87642278ede6 191 }
hudakz 0:87642278ede6 192
hudakz 0:87642278ede6 193 // Open the numbers file
Joeatsumi 4:21a356ae0747 194 pc.printf("Opening \"/fs/log.csv\"... ");
hudakz 0:87642278ede6 195 fflush(stdout);
hudakz 0:87642278ede6 196
Joeatsumi 4:21a356ae0747 197 FILE* f = fopen("/fs/log.csv", "r+");
Joeatsumi 4:21a356ae0747 198 FILE* fp = fopen("/fs/log.txt", "r+");
Joeatsumi 4:21a356ae0747 199
Joeatsumi 2:e6496a794bde 200 pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
Joeatsumi 2:e6496a794bde 201
hudakz 0:87642278ede6 202 if (!f) {
hudakz 0:87642278ede6 203 // Create the numbers file if it doesn't exist
Joeatsumi 2:e6496a794bde 204 pc.printf("No file found, creating a new file... ");
hudakz 0:87642278ede6 205 fflush(stdout);
Joeatsumi 4:21a356ae0747 206 f = fopen("/fs/log.csv", "w+");
Joeatsumi 4:21a356ae0747 207 fp = fopen("/fs/log.txt", "w+");
Joeatsumi 4:21a356ae0747 208
Joeatsumi 2:e6496a794bde 209 pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 210 }
Joeatsumi 4:21a356ae0747 211
Joeatsumi 2:e6496a794bde 212
Joeatsumi 2:e6496a794bde 213
Joeatsumi 2:e6496a794bde 214
Joeatsumi 4:21a356ae0747 215 /*define period of serve and ESC*/
Joeatsumi 4:21a356ae0747 216 ESC.period(SERVO_PERIOD);
Joeatsumi 4:21a356ae0747 217 Elevator_servo.period(SERVO_PERIOD);
Joeatsumi 4:21a356ae0747 218 Rudder_servo.period(SERVO_PERIOD);
Joeatsumi 4:21a356ae0747 219 Aileron_R_servo.period(SERVO_PERIOD);
Joeatsumi 4:21a356ae0747 220 Aileron_L_servo.period(SERVO_PERIOD);
Joeatsumi 4:21a356ae0747 221 /*------------------------------*/
Joeatsumi 4:21a356ae0747 222 Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
Joeatsumi 4:21a356ae0747 223 Rudder_servo.pulsewidth(NUTRAL);
Joeatsumi 4:21a356ae0747 224 Aileron_R_servo.pulsewidth(NUTRAL);
Joeatsumi 4:21a356ae0747 225 Aileron_L_servo.pulsewidth(NUTRAL);
Joeatsumi 4:21a356ae0747 226
Joeatsumi 4:21a356ae0747 227 Activate_ESC();//Activate ESC
Joeatsumi 4:21a356ae0747 228
Joeatsumi 4:21a356ae0747 229 /*--------------ピン変化割り込みに対応した関数の宣言--------------*/
Joeatsumi 4:21a356ae0747 230 button.fall(&flip);//GPSモジュールからの1pps信号を立ち下がり割り込みで読み取る。
Joeatsumi 4:21a356ae0747 231
Joeatsumi 4:21a356ae0747 232 input_from_throttle.rise(&Input_throttle);//
Joeatsumi 4:21a356ae0747 233
Joeatsumi 4:21a356ae0747 234 input_from_Aileron_1.fall(&Input_Aileron_1_fall);
Joeatsumi 4:21a356ae0747 235 input_from_elevator.fall(&Input_elevator_fall);//
Joeatsumi 4:21a356ae0747 236 input_from_throttle.fall(&Input_throttle_fall);//
Joeatsumi 4:21a356ae0747 237 input_from_rudder.fall(&Input_rudder_fall);//
Joeatsumi 4:21a356ae0747 238 input_from_gear.fall(&Input_gear_fall);
Joeatsumi 4:21a356ae0747 239 input_from_Aileron_2.fall(&Input_Aileron_2_fall);//
Joeatsumi 4:21a356ae0747 240 input_from_pitch.fall(&Input_pitch_fall);//
Joeatsumi 4:21a356ae0747 241 input_from_AUX5.fall(&Input_AUX5_fall);//
Joeatsumi 4:21a356ae0747 242
Joeatsumi 4:21a356ae0747 243 ch_time.start();//時間計測スタート
Joeatsumi 4:21a356ae0747 244 /*----------------------------------------------------------*/
Joeatsumi 4:21a356ae0747 245
Joeatsumi 4:21a356ae0747 246 pc.baud(115200);
Joeatsumi 4:21a356ae0747 247 gps.baud(115200);
Joeatsumi 4:21a356ae0747 248
Joeatsumi 4:21a356ae0747 249 //gps.attach(gps_rx, Serial::RxIrq);//シリアル割り込み
Joeatsumi 4:21a356ae0747 250 //pc.attach(gps_rx, Serial::RxIrq);//シリアル割り込み
Joeatsumi 4:21a356ae0747 251
Joeatsumi 4:21a356ae0747 252 passed_time.start();//タイマー開始
Joeatsumi 4:21a356ae0747 253 Time_old=float(passed_time.read());
Joeatsumi 4:21a356ae0747 254
Joeatsumi 4:21a356ae0747 255 /*初期姿勢角の算出*/
Joeatsumi 4:21a356ae0747 256 scan_update(sensor_array);//角速度、加速度を求める
Joeatsumi 4:21a356ae0747 257 Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll);
Joeatsumi 4:21a356ae0747 258 Gyro_Pitch_Roll[0]=Acc_Pitch_Roll[0];//初期姿勢角の算出 Pitch
Joeatsumi 4:21a356ae0747 259 Gyro_Pitch_Roll[1]=Acc_Pitch_Roll[1];//初期姿勢角の算出 Roll
Joeatsumi 4:21a356ae0747 260
Joeatsumi 4:21a356ae0747 261 pc.printf("Start!!\r\n");
Joeatsumi 4:21a356ae0747 262
Joeatsumi 4:21a356ae0747 263 while(1) {
Joeatsumi 4:21a356ae0747 264
Joeatsumi 4:21a356ae0747 265 switch (GPS_interruptIn) {
Joeatsumi 4:21a356ae0747 266 case 1:
Joeatsumi 4:21a356ae0747 267
Joeatsumi 4:21a356ae0747 268 gps_rx();
Joeatsumi 4:21a356ae0747 269 gps_read();
Joeatsumi 4:21a356ae0747 270 GPS_interruptIn--;
Joeatsumi 4:21a356ae0747 271 break;
Joeatsumi 2:e6496a794bde 272
Joeatsumi 4:21a356ae0747 273 default:
Joeatsumi 2:e6496a794bde 274
Joeatsumi 4:21a356ae0747 275 sensor_update();
Joeatsumi 4:21a356ae0747 276 break;
Joeatsumi 4:21a356ae0747 277 }//switch end
Joeatsumi 4:21a356ae0747 278
Joeatsumi 4:21a356ae0747 279 /*
Joeatsumi 4:21a356ae0747 280 ここでプロポの信号のパルス幅を読み取って、手動or 自動の切り替えを行う必要がある。
Joeatsumi 4:21a356ae0747 281 以下のpitch control等は自動操縦とみなしてよい。
Joeatsumi 4:21a356ae0747 282 */
Joeatsumi 4:21a356ae0747 283
Joeatsumi 4:21a356ae0747 284
Joeatsumi 4:21a356ae0747 285 if(switch_duty>THRESHOLD){//オートパイロット
Joeatsumi 2:e6496a794bde 286
Joeatsumi 4:21a356ae0747 287 //Pitch control
Joeatsumi 4:21a356ae0747 288 //pitch_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old));
Joeatsumi 4:21a356ae0747 289 elevator_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old));
Joeatsumi 4:21a356ae0747 290 Elevator_servo.pulsewidth(elevator_duty);
Joeatsumi 2:e6496a794bde 291
Joeatsumi 4:21a356ae0747 292 //roll control
Joeatsumi 4:21a356ae0747 293 //roll_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old))
Joeatsumi 4:21a356ae0747 294 aileron_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old));
Joeatsumi 2:e6496a794bde 295
Joeatsumi 4:21a356ae0747 296 //角速度だけフィードバック
Joeatsumi 4:21a356ae0747 297 rudder_duty=NUTRAL+PID_duty_rudder(0.0,0.0,(Time_new-Time_old),omega_z);
Joeatsumi 4:21a356ae0747 298 ///pc.printf("%f\r\n",rudder_duty);
Joeatsumi 2:e6496a794bde 299
Joeatsumi 2:e6496a794bde 300
Joeatsumi 4:21a356ae0747 301 //スロットルはマニュアル
Joeatsumi 4:21a356ae0747 302 //Servo_command
Joeatsumi 4:21a356ae0747 303 if( (float(passed_time.read())-Servo_command)>0.003 ){
Joeatsumi 2:e6496a794bde 304
Joeatsumi 4:21a356ae0747 305 ESC.pulsewidth(throttle_duty);
Joeatsumi 4:21a356ae0747 306 Elevator_servo.pulsewidth(elevator_duty);
Joeatsumi 4:21a356ae0747 307 Rudder_servo.pulsewidth(rudder_duty);
Joeatsumi 4:21a356ae0747 308 Aileron_R_servo.pulsewidth(aileron_duty);
Joeatsumi 4:21a356ae0747 309 Aileron_L_servo.pulsewidth(aileron_duty);
Joeatsumi 2:e6496a794bde 310
Joeatsumi 4:21a356ae0747 311 Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新
Joeatsumi 4:21a356ae0747 312 }else{}
Joeatsumi 4:21a356ae0747 313
Joeatsumi 4:21a356ae0747 314 }else{//マニュアルモード
Joeatsumi 4:21a356ae0747 315
Joeatsumi 4:21a356ae0747 316 if( (float(passed_time.read())-Servo_command)>0.003 ){
Joeatsumi 4:21a356ae0747 317
Joeatsumi 4:21a356ae0747 318 ESC.pulsewidth(throttle_duty);
Joeatsumi 4:21a356ae0747 319 Elevator_servo.pulsewidth(elevator_duty);
Joeatsumi 4:21a356ae0747 320 Rudder_servo.pulsewidth(rudder_duty);
Joeatsumi 4:21a356ae0747 321 Aileron_R_servo.pulsewidth(aileron_duty);
Joeatsumi 4:21a356ae0747 322 Aileron_L_servo.pulsewidth(aileron_duty);
Joeatsumi 4:21a356ae0747 323
Joeatsumi 4:21a356ae0747 324 Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新
Joeatsumi 4:21a356ae0747 325
Joeatsumi 4:21a356ae0747 326 }else{}
Joeatsumi 2:e6496a794bde 327
Joeatsumi 4:21a356ae0747 328 }
Joeatsumi 4:21a356ae0747 329
Joeatsumi 4:21a356ae0747 330 pc.printf("%f,%f,%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps,latitude_gps,longtude_gps);
Joeatsumi 4:21a356ae0747 331 err = fprintf(f,"%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps);
Joeatsumi 4:21a356ae0747 332 err = fprintf(fp,"%f,%f\r\n",latitude_gps,longtude_gps);
Joeatsumi 4:21a356ae0747 333
Joeatsumi 4:21a356ae0747 334 Time_old=Time_new;//時間更新
Joeatsumi 4:21a356ae0747 335
Joeatsumi 4:21a356ae0747 336 if(switch_off==1){
Joeatsumi 4:21a356ae0747 337 //fclose(fp);
Joeatsumi 4:21a356ae0747 338 //flipper.detach();
Joeatsumi 4:21a356ae0747 339
Joeatsumi 4:21a356ae0747 340 err = fclose(f);
Joeatsumi 4:21a356ae0747 341 err = fclose(fp);
Joeatsumi 4:21a356ae0747 342
Joeatsumi 4:21a356ae0747 343 pc.printf("Writing finish!");
Joeatsumi 4:21a356ae0747 344 myled=0;
Joeatsumi 4:21a356ae0747 345 break;
Joeatsumi 4:21a356ae0747 346
Joeatsumi 4:21a356ae0747 347 }
Joeatsumi 4:21a356ae0747 348
Joeatsumi 4:21a356ae0747 349 //wait(0.002);//この値は10倍されると考える
Joeatsumi 4:21a356ae0747 350 }//while ends
Joeatsumi 4:21a356ae0747 351
Joeatsumi 4:21a356ae0747 352 }//main ends
Joeatsumi 4:21a356ae0747 353
Joeatsumi 4:21a356ae0747 354 void gps_rx(){
Joeatsumi 4:21a356ae0747 355
Joeatsumi 4:21a356ae0747 356 // __disable_irq(); // 割り込み禁止
Joeatsumi 4:21a356ae0747 357
Joeatsumi 4:21a356ae0747 358 int i=0;
Joeatsumi 4:21a356ae0747 359
Joeatsumi 4:21a356ae0747 360 while(gps.getc()!='$'){
Joeatsumi 4:21a356ae0747 361 }
Joeatsumi 4:21a356ae0747 362 while( (NMEA_sentense[i]=gps.getc()) != '\r'){
Joeatsumi 4:21a356ae0747 363 //while( (NMEA_sentense[i]=pc.getc()) != '\r'){
Joeatsumi 4:21a356ae0747 364 //pc.putc(NMEA_sentense[i]);
Joeatsumi 4:21a356ae0747 365 i++;
Joeatsumi 4:21a356ae0747 366 if(i==READBUFFERSIZE){
Joeatsumi 4:21a356ae0747 367 i=(READBUFFERSIZE-1);
Joeatsumi 4:21a356ae0747 368 break;
Joeatsumi 4:21a356ae0747 369 }
Joeatsumi 4:21a356ae0747 370 }
Joeatsumi 4:21a356ae0747 371 NMEA_sentense[i]='\0';
Joeatsumi 4:21a356ae0747 372 //pc.printf("\r\n");
Joeatsumi 4:21a356ae0747 373
Joeatsumi 4:21a356ae0747 374 if(GPS_flag_update==0){
Joeatsumi 4:21a356ae0747 375 GPS_flag_update++;
Joeatsumi 4:21a356ae0747 376 }
Joeatsumi 4:21a356ae0747 377
Joeatsumi 4:21a356ae0747 378 //__enable_irq(); // 割り込み許可
Joeatsumi 4:21a356ae0747 379
Joeatsumi 4:21a356ae0747 380 }//void gps_rx
Joeatsumi 4:21a356ae0747 381
Joeatsumi 4:21a356ae0747 382 void gps_read(){
Joeatsumi 4:21a356ae0747 383 /*------gps のNMEAフォーマットの処理プログラム----------------*/
Joeatsumi 4:21a356ae0747 384 float temp=0.0, deg=0.0, min=0.0;
Joeatsumi 2:e6496a794bde 385
Joeatsumi 4:21a356ae0747 386 char *pszTime;//UTC時刻
Joeatsumi 4:21a356ae0747 387 char* pszLat;////緯度
Joeatsumi 4:21a356ae0747 388 char* pszLong;//経度
Joeatsumi 4:21a356ae0747 389 char* pszSp;//速度
Joeatsumi 4:21a356ae0747 390 char* pszSpd;//速度方向
Joeatsumi 4:21a356ae0747 391 char* pszDate;//日付
Joeatsumi 4:21a356ae0747 392
Joeatsumi 4:21a356ae0747 393 //strtokで処理した文字列は内容がNullになるので、テストでは、毎回NMEA_sentense[128]を初期化しないといけない
Joeatsumi 4:21a356ae0747 394 //char NMEA_sentense[128]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
Joeatsumi 4:21a356ae0747 395
Joeatsumi 4:21a356ae0747 396 //Time_old=float(passed_time.read());
Joeatsumi 4:21a356ae0747 397
Joeatsumi 4:21a356ae0747 398
Joeatsumi 4:21a356ae0747 399
Joeatsumi 4:21a356ae0747 400 if((GPS_flag_update==1)&&(0==strncmp( "GPRMC", NMEA_sentense, 5 ))){//GPS_flag_updateも判定に加えた方がいい
Joeatsumi 4:21a356ae0747 401 //if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){//GPS_flag_updateも判定に加えた方がいい
Joeatsumi 4:21a356ae0747 402
Joeatsumi 4:21a356ae0747 403 strtok( NMEA_sentense, DELIMITER ); // $GPRMC
Joeatsumi 4:21a356ae0747 404 pszTime = strtok( NULL, DELIMITER ); // UTC時刻
Joeatsumi 4:21a356ae0747 405 strtok( NULL, DELIMITER ); // ステータス
Joeatsumi 4:21a356ae0747 406 pszLat = strtok( NULL, DELIMITER ); // 緯度(dddmm.mmmm)
Joeatsumi 4:21a356ae0747 407 strtok( NULL, DELIMITER ); // 北緯か南緯か
Joeatsumi 4:21a356ae0747 408 pszLong = strtok( NULL, DELIMITER ); // 経度(dddmm.mmmm)
Joeatsumi 4:21a356ae0747 409 strtok( NULL, DELIMITER ); // 東経か西経か
Joeatsumi 4:21a356ae0747 410
Joeatsumi 4:21a356ae0747 411 pszSp = strtok( NULL, DELIMITER ); // 速度（vvv.v）
Joeatsumi 4:21a356ae0747 412 pszSpd = strtok( NULL, DELIMITER ); // 速度方向（ddd.d）
Joeatsumi 4:21a356ae0747 413 pszDate = strtok( NULL, DELIMITER ); // 日付
Joeatsumi 4:21a356ae0747 414
Joeatsumi 4:21a356ae0747 415 if( NULL != pszLong ){//pszLongがnullでないのならば
Joeatsumi 4:21a356ae0747 416 temp = atof(pszLat);
Joeatsumi 4:21a356ae0747 417 deg = (int)(temp/100);
Joeatsumi 4:21a356ae0747 418 min = temp - deg * 100;
Joeatsumi 4:21a356ae0747 419 latitude_gps = deg + min / 60;//latitudeの算出
Joeatsumi 4:21a356ae0747 420
Joeatsumi 4:21a356ae0747 421 temp = atof(pszLong);
Joeatsumi 4:21a356ae0747 422 deg = (int)(temp/100);
Joeatsumi 4:21a356ae0747 423 min = temp - deg * 100;
Joeatsumi 4:21a356ae0747 424 longtude_gps = deg + min / 60;//longtudeの算出
Joeatsumi 4:21a356ae0747 425
Joeatsumi 4:21a356ae0747 426 speed_gps=atof(pszSp)*KNOT_TO_METER_PER_SEC;
Joeatsumi 4:21a356ae0747 427
Joeatsumi 4:21a356ae0747 428 azimuth_gps=atof(pszSpd);
Joeatsumi 4:21a356ae0747 429
Joeatsumi 4:21a356ae0747 430 GPS_flag_update--;
Joeatsumi 4:21a356ae0747 431
Joeatsumi 4:21a356ae0747 432 //pc.printf("%f,%f\r\n",longtude_gps,latitude_gps);
Joeatsumi 4:21a356ae0747 433
Joeatsumi 4:21a356ae0747 434 }else{}//end of ( NULL != pszLong )
Joeatsumi 4:21a356ae0747 435
Joeatsumi 4:21a356ae0747 436 }else{}//end of if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){
Joeatsumi 4:21a356ae0747 437 }
Joeatsumi 4:21a356ae0747 438
Joeatsumi 4:21a356ae0747 439
Joeatsumi 4:21a356ae0747 440 void sensor_update(){
Joeatsumi 4:21a356ae0747 441 scan_update(sensor_array);//角速度、加速度を求める
Joeatsumi 4:21a356ae0747 442
Joeatsumi 4:21a356ae0747 443 Time_new=float(passed_time.read());//時間の更新
Joeatsumi 4:21a356ae0747 444
Joeatsumi 4:21a356ae0747 445 Gyro_Pitch_Roll[0]+=sensor_array[1]*(Time_new-Time_old);//初期姿勢角の算出 Pitch
Joeatsumi 4:21a356ae0747 446 Gyro_Pitch_Roll[1]+=sensor_array[0]*(Time_new-Time_old);//初期姿勢角の算出 Roll
Joeatsumi 4:21a356ae0747 447
Joeatsumi 4:21a356ae0747 448
Joeatsumi 4:21a356ae0747 449 /*--------------------加速センサでジャイロの値を補正する --------------------*/
Joeatsumi 4:21a356ae0747 450
Joeatsumi 4:21a356ae0747 451
Joeatsumi 4:21a356ae0747 452
Joeatsumi 4:21a356ae0747 453 if(9.8065*1.05 > sqrt((sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5])))
Joeatsumi 4:21a356ae0747 454 {
Joeatsumi 4:21a356ae0747 455 Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll);
Joeatsumi 4:21a356ae0747 456 Gyro_Pitch_Roll[0]*=0.95;//
Joeatsumi 4:21a356ae0747 457 Gyro_Pitch_Roll[1]*=0.95;//
Joeatsumi 4:21a356ae0747 458 Gyro_Pitch_Roll[0]+=(0.05*Acc_Pitch_Roll[0]);//
Joeatsumi 4:21a356ae0747 459 Gyro_Pitch_Roll[1]+=(0.05*Acc_Pitch_Roll[1]);//
Joeatsumi 4:21a356ae0747 460
Joeatsumi 4:21a356ae0747 461
Joeatsumi 4:21a356ae0747 462 }else{}
Joeatsumi 4:21a356ae0747 463
Joeatsumi 4:21a356ae0747 464 //pc.printf("%f,%f,%f\r\n",Time_new,Acc_Pitch_Roll[0]*RAD_TO_DEG,Acc_Pitch_Roll[1]*RAD_TO_DEG);
Joeatsumi 4:21a356ae0747 465 //pc.printf("%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1]);
Joeatsumi 4:21a356ae0747 466
Joeatsumi 4:21a356ae0747 467 //Time_old=Time_new;
Joeatsumi 4:21a356ae0747 468
Joeatsumi 4:21a356ae0747 469 }
Joeatsumi 4:21a356ae0747 470 double Degree_to_PWM_duty(double degree){
Joeatsumi 4:21a356ae0747 471
Joeatsumi 4:21a356ae0747 472 double duty=0.0;
Joeatsumi 4:21a356ae0747 473
Joeatsumi 4:21a356ae0747 474 duty=(0.2/1000)/20.0*degree;
Joeatsumi 4:21a356ae0747 475
Joeatsumi 4:21a356ae0747 476 if((duty+NUTRAL)>=UPPER_DUTY){
Joeatsumi 4:21a356ae0747 477 duty=UPPER_DUTY-NUTRAL;
Joeatsumi 4:21a356ae0747 478 }else if((duty+NUTRAL)<=LOWER_DUTY){
Joeatsumi 4:21a356ae0747 479 duty=NUTRAL-LOWER_DUTY;
Joeatsumi 4:21a356ae0747 480 }else{}
Joeatsumi 4:21a356ae0747 481
Joeatsumi 4:21a356ae0747 482 return duty;
Joeatsumi 4:21a356ae0747 483
Joeatsumi 4:21a356ae0747 484 }
Joeatsumi 4:21a356ae0747 485
Joeatsumi 4:21a356ae0747 486 double PID_duty_pitch(double target,double vol,float dt){
Joeatsumi 4:21a356ae0747 487 //ここで角度の単位で偏差は入力される
Joeatsumi 4:21a356ae0747 488 //出力はPWMで
Joeatsumi 4:21a356ae0747 489 double duty=0.0;
Joeatsumi 4:21a356ae0747 490 double add_duty=0.0;
Joeatsumi 4:21a356ae0747 491
Joeatsumi 4:21a356ae0747 492 Prop_p=target-vol;
Joeatsumi 4:21a356ae0747 493
Joeatsumi 4:21a356ae0747 494 //pc.printf("%f/r/n",Prop_p);
Joeatsumi 4:21a356ae0747 495
Joeatsumi 4:21a356ae0747 496 Int_p+=Prop_p*double(dt);
Joeatsumi 4:21a356ae0747 497 Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
Joeatsumi 4:21a356ae0747 498
Joeatsumi 4:21a356ae0747 499 Pre_Prop_p=Prop_p;
Joeatsumi 4:21a356ae0747 500
Joeatsumi 4:21a356ae0747 501 //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
Joeatsumi 4:21a356ae0747 502
Joeatsumi 4:21a356ae0747 503 add_duty=1.0*Prop_p;
Joeatsumi 4:21a356ae0747 504
Joeatsumi 4:21a356ae0747 505 duty+=add_duty;
Joeatsumi 4:21a356ae0747 506 duty=Degree_to_PWM_duty(duty);
Joeatsumi 4:21a356ae0747 507
Joeatsumi 4:21a356ae0747 508 return duty;
Joeatsumi 4:21a356ae0747 509
Joeatsumi 4:21a356ae0747 510 }
Joeatsumi 4:21a356ae0747 511
Joeatsumi 4:21a356ae0747 512 double PID_duty_rudder(double target,double vol,float dt,float yaw_rate){
Joeatsumi 4:21a356ae0747 513 //ここで角度の単位で偏差は入力される
Joeatsumi 4:21a356ae0747 514 //出力はPWMで
Joeatsumi 4:21a356ae0747 515 double duty=0.0;
Joeatsumi 4:21a356ae0747 516 double add_duty=0.0;
Joeatsumi 4:21a356ae0747 517
Joeatsumi 4:21a356ae0747 518 Prop_p=target-vol;
Joeatsumi 4:21a356ae0747 519
Joeatsumi 4:21a356ae0747 520 //pc.printf("%f/r/n",Prop_p);
Joeatsumi 4:21a356ae0747 521
Joeatsumi 4:21a356ae0747 522 Int_p+=Prop_p*double(dt);
Joeatsumi 4:21a356ae0747 523 Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
Joeatsumi 4:21a356ae0747 524
Joeatsumi 4:21a356ae0747 525 Pre_Prop_p=Prop_p;
Joeatsumi 4:21a356ae0747 526
Joeatsumi 4:21a356ae0747 527 //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
Joeatsumi 3:3fa7882a5fd0 528
Joeatsumi 4:21a356ae0747 529 //add_duty=1.0*Prop_p;
Joeatsumi 4:21a356ae0747 530 //今回は角速度に反応させてラダーを動かす
Joeatsumi 4:21a356ae0747 531 add_duty=double(yaw_rate)*0.1;
Joeatsumi 4:21a356ae0747 532
Joeatsumi 4:21a356ae0747 533 duty+=add_duty;
Joeatsumi 4:21a356ae0747 534 duty=Degree_to_PWM_duty(duty);
Joeatsumi 4:21a356ae0747 535
Joeatsumi 4:21a356ae0747 536 return duty;
Joeatsumi 4:21a356ae0747 537
Joeatsumi 4:21a356ae0747 538 }
Joeatsumi 4:21a356ae0747 539
Joeatsumi 4:21a356ae0747 540 double PID_duty_roll(double target,double vol,float dt){
Joeatsumi 4:21a356ae0747 541 //ここで角度の単位で偏差は入力される
Joeatsumi 4:21a356ae0747 542 //出力はPWMで
Joeatsumi 4:21a356ae0747 543 double duty=0.0;
Joeatsumi 4:21a356ae0747 544 double add_duty=0.0;
Joeatsumi 4:21a356ae0747 545
Joeatsumi 4:21a356ae0747 546 Prop_p=target-vol;
Joeatsumi 4:21a356ae0747 547
Joeatsumi 4:21a356ae0747 548 //pc.printf("%f/r/n",Prop_p);
Joeatsumi 4:21a356ae0747 549
Joeatsumi 4:21a356ae0747 550 Int_p+=Prop_p*double(dt);
Joeatsumi 4:21a356ae0747 551 Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
Joeatsumi 4:21a356ae0747 552
Joeatsumi 4:21a356ae0747 553 Pre_Prop_p=Prop_p;
Joeatsumi 4:21a356ae0747 554
Joeatsumi 4:21a356ae0747 555 //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
Joeatsumi 4:21a356ae0747 556
Joeatsumi 4:21a356ae0747 557 add_duty=1.0*Prop_p;
Joeatsumi 4:21a356ae0747 558
Joeatsumi 4:21a356ae0747 559 duty+=add_duty;
Joeatsumi 4:21a356ae0747 560 duty=Degree_to_PWM_duty(duty);
Joeatsumi 4:21a356ae0747 561
Joeatsumi 4:21a356ae0747 562 return duty;
Joeatsumi 3:3fa7882a5fd0 563
Joeatsumi 4:21a356ae0747 564 }
Joeatsumi 4:21a356ae0747 565 void Initialize_ESC(){
Joeatsumi 4:21a356ae0747 566
Joeatsumi 4:21a356ae0747 567 ESC.pulsewidth(0.002);
Joeatsumi 4:21a356ae0747 568 wait(3);
Joeatsumi 4:21a356ae0747 569
Joeatsumi 4:21a356ae0747 570 ESC.pulsewidth(0.001);
Joeatsumi 4:21a356ae0747 571 wait(5);
Joeatsumi 4:21a356ae0747 572
Joeatsumi 4:21a356ae0747 573 }
Joeatsumi 4:21a356ae0747 574
Joeatsumi 4:21a356ae0747 575
Joeatsumi 4:21a356ae0747 576 void Activate_ESC(){
Joeatsumi 4:21a356ae0747 577
Joeatsumi 4:21a356ae0747 578 ESC.pulsewidth(0.001);
Joeatsumi 4:21a356ae0747 579 wait(5);
Joeatsumi 4:21a356ae0747 580
Joeatsumi 4:21a356ae0747 581 }
Joeatsumi 4:21a356ae0747 582
Joeatsumi 4:21a356ae0747 583 void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]){
Joeatsumi 4:21a356ae0747 584
Joeatsumi 4:21a356ae0747 585 float Roll_before = (y_acc)/(z_acc);
Joeatsumi 4:21a356ae0747 586 float Roll = atan(Roll_before);
Joeatsumi 3:3fa7882a5fd0 587
Joeatsumi 4:21a356ae0747 588 float Pitch_before = (x_acc/sqrt((y_acc*y_acc+z_acc*z_acc)) );
Joeatsumi 4:21a356ae0747 589 float Pitch = -atan(Pitch_before);
Joeatsumi 4:21a356ae0747 590
Joeatsumi 4:21a356ae0747 591 qua[0]=Pitch*RAD_TO_DEG;
Joeatsumi 4:21a356ae0747 592 qua[1]=Roll*RAD_TO_DEG;
Joeatsumi 4:21a356ae0747 593 //float Acc_Pitch_Roll[2];
Joeatsumi 4:21a356ae0747 594
Joeatsumi 4:21a356ae0747 595 }
Joeatsumi 4:21a356ae0747 596
Joeatsumi 4:21a356ae0747 597 void scan_update(float box_sensor[6]){
Joeatsumi 4:21a356ae0747 598
Joeatsumi 4:21a356ae0747 599 float a[3];//加速度の値
Joeatsumi 4:21a356ae0747 600 float g[3];//ジャイロの値[rad/s]
Joeatsumi 4:21a356ae0747 601
Joeatsumi 4:21a356ae0747 602 //int16_t raw_compass[3];//地磁気センサの値
Joeatsumi 4:21a356ae0747 603
Joeatsumi 4:21a356ae0747 604 mpu.setAcceleroRange(0);//acceleration range is +-4G
Joeatsumi 4:21a356ae0747 605 mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
Joeatsumi 4:21a356ae0747 606
Joeatsumi 4:21a356ae0747 607 mpu.getAccelero(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis;
Joeatsumi 4:21a356ae0747 608 mpu.getGyro(g);// 角速度を格納する配列[rad/s]
Joeatsumi 4:21a356ae0747 609
Joeatsumi 4:21a356ae0747 610 //pc.printf("%f,%f,%f\r\n",g[0],g[1],g[2]);
Joeatsumi 4:21a356ae0747 611
Joeatsumi 4:21a356ae0747 612 //compass.getXYZ(raw_compass);//地磁気の値を格納する配列
Joeatsumi 4:21a356ae0747 613
Joeatsumi 4:21a356ae0747 614 box_sensor[0]=((-g[0])-(GYRO_FIX_X/7505.7))*RAD_TO_DEG;
Joeatsumi 4:21a356ae0747 615 box_sensor[1]=((g[1])-(GYRO_FIX_Y/7505.7))*RAD_TO_DEG;
Joeatsumi 4:21a356ae0747 616 box_sensor[2]=((-g[2])-(GYRO_FIX_Z/7505.7))*RAD_TO_DEG;
Joeatsumi 4:21a356ae0747 617
Joeatsumi 4:21a356ae0747 618 omega_z=box_sensor[2];
Joeatsumi 4:21a356ae0747 619
Joeatsumi 4:21a356ae0747 620 box_sensor[3]=a[0];
Joeatsumi 4:21a356ae0747 621 box_sensor[4]=-a[1];
Joeatsumi 4:21a356ae0747 622 box_sensor[5]=a[2];
Joeatsumi 4:21a356ae0747 623
Joeatsumi 4:21a356ae0747 624 //pc.printf("%f,%f,%f\r\n",box_sensor[0],box_sensor[1],box_sensor[2]);
Joeatsumi 4:21a356ae0747 625 }
Joeatsumi 4:21a356ae0747 626
Joeatsumi 4:21a356ae0747 627 void flip(){
Joeatsumi 4:21a356ae0747 628
Joeatsumi 4:21a356ae0747 629 GPS_interruptIn++;
Joeatsumi 4:21a356ae0747 630
Joeatsumi 4:21a356ae0747 631 };
Joeatsumi 4:21a356ae0747 632
Joeatsumi 4:21a356ae0747 633
Joeatsumi 4:21a356ae0747 634 //ch1
Joeatsumi 4:21a356ae0747 635 void Input_Aileron_1(){
Joeatsumi 4:21a356ae0747 636
Joeatsumi 4:21a356ae0747 637 first_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 638 //pc.printf("ch1=%f",second_period);
Joeatsumi 4:21a356ae0747 639 };
Joeatsumi 4:21a356ae0747 640
Joeatsumi 4:21a356ae0747 641 void Input_Aileron_1_fall(){
Joeatsumi 4:21a356ae0747 642 first_period=ch_time.read();
Joeatsumi 4:21a356ae0747 643 aileron_duty=first_period-first_period_old;
Joeatsumi 4:21a356ae0747 644 };
Joeatsumi 4:21a356ae0747 645
Joeatsumi 4:21a356ae0747 646
Joeatsumi 4:21a356ae0747 647 //ch2
Joeatsumi 4:21a356ae0747 648 void Input_elevator(){
Joeatsumi 4:21a356ae0747 649 second_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 650 };
hudakz 0:87642278ede6 651
Joeatsumi 4:21a356ae0747 652 void Input_elevator_fall(){
Joeatsumi 4:21a356ae0747 653 second_period=ch_time.read();
Joeatsumi 4:21a356ae0747 654 elevator_duty=second_period-second_period_old;
Joeatsumi 4:21a356ae0747 655 };
Joeatsumi 4:21a356ae0747 656
Joeatsumi 4:21a356ae0747 657
Joeatsumi 4:21a356ae0747 658 //ch3
Joeatsumi 4:21a356ae0747 659 void Input_throttle(){
Joeatsumi 4:21a356ae0747 660
Joeatsumi 4:21a356ae0747 661 first_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 662 second_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 663 third_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 664 fourth_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 665 fifth_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 666 sixth_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 667
Joeatsumi 4:21a356ae0747 668
Joeatsumi 4:21a356ae0747 669 }
Joeatsumi 4:21a356ae0747 670
Joeatsumi 4:21a356ae0747 671 void Input_throttle_fall(){
Joeatsumi 4:21a356ae0747 672 third_period=ch_time.read();
Joeatsumi 4:21a356ae0747 673
Joeatsumi 4:21a356ae0747 674 /*デコード*/
Joeatsumi 4:21a356ae0747 675 throttle_duty=third_period-third_period_old;
Joeatsumi 4:21a356ae0747 676 //rudder_duty=fourth_period-fourth_period_old;
Joeatsumi 4:21a356ae0747 677 //elevator_duty=second_period-second_period_old;
Joeatsumi 4:21a356ae0747 678 //aileron_duty=first_period-first_period_old;
Joeatsumi 4:21a356ae0747 679
Joeatsumi 4:21a356ae0747 680 //pc.printf("throttle_duty=%f:rudder_duty=%f:elevator_duty=%f\r\n",throttle_duty,rudder_duty,elevator_duty);
Joeatsumi 4:21a356ae0747 681 }
Joeatsumi 4:21a356ae0747 682
Joeatsumi 4:21a356ae0747 683 //ch4
Joeatsumi 4:21a356ae0747 684 void Input_rudder(){
Joeatsumi 4:21a356ae0747 685 fourth_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 686 }
Joeatsumi 4:21a356ae0747 687
Joeatsumi 4:21a356ae0747 688 void Input_rudder_fall(){
Joeatsumi 4:21a356ae0747 689 fourth_period=ch_time.read();
Joeatsumi 4:21a356ae0747 690 rudder_duty=fourth_period-fourth_period_old;
Joeatsumi 4:21a356ae0747 691 }
Joeatsumi 4:21a356ae0747 692
Joeatsumi 4:21a356ae0747 693 //ch5
Joeatsumi 4:21a356ae0747 694 void Input_gear(){
Joeatsumi 4:21a356ae0747 695 fifth_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 696
Joeatsumi 4:21a356ae0747 697 }
Joeatsumi 4:21a356ae0747 698
Joeatsumi 4:21a356ae0747 699 void Input_gear_fall(){
Joeatsumi 4:21a356ae0747 700 fifth_period=ch_time.read();
Joeatsumi 4:21a356ae0747 701 switch_duty=fifth_period-fifth_period_old;
Joeatsumi 4:21a356ae0747 702 }
Joeatsumi 4:21a356ae0747 703
Joeatsumi 4:21a356ae0747 704 //ch6
Joeatsumi 4:21a356ae0747 705 void Input_Aileron_2(){
Joeatsumi 4:21a356ae0747 706 sixth_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 707 }
Joeatsumi 4:21a356ae0747 708
Joeatsumi 4:21a356ae0747 709 void Input_Aileron_2_fall(){
Joeatsumi 4:21a356ae0747 710 sixth_period=ch_time.read();
Joeatsumi 4:21a356ae0747 711 };
Joeatsumi 4:21a356ae0747 712
Joeatsumi 4:21a356ae0747 713 //ch7
Joeatsumi 4:21a356ae0747 714 void Input_pitch(){
Joeatsumi 4:21a356ae0747 715 seventh_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 716 }
Joeatsumi 4:21a356ae0747 717
Joeatsumi 4:21a356ae0747 718 void Input_pitch_fall(){
Joeatsumi 4:21a356ae0747 719 seventh_period=ch_time.read();
Joeatsumi 4:21a356ae0747 720 }
Joeatsumi 4:21a356ae0747 721
Joeatsumi 4:21a356ae0747 722 //ch8
Joeatsumi 4:21a356ae0747 723 void Input_AUX5(){
Joeatsumi 4:21a356ae0747 724 eighth_period_old=ch_time.read();
Joeatsumi 4:21a356ae0747 725 }
Joeatsumi 4:21a356ae0747 726
Joeatsumi 4:21a356ae0747 727 void Input_AUX5_fall(){
Joeatsumi 4:21a356ae0747 728 eighth_period=ch_time.read();
Joeatsumi 4:21a356ae0747 729 }