Atsumi Toda
UAVの姿勢推定に使用するプログラム。
Diff: main.cpp
- Revision:
- 4:21a356ae0747
- Parent:
- 3:3fa7882a5fd0
--- a/main.cpp Wed Jul 24 12:00:01 2019 +0000
+++ b/main.cpp Mon Aug 19 02:50:13 2019 +0000
@@ -1,104 +1,84 @@
+/*
+This system is vertion 2.0. Thsi program enable us to read signals from receiver ,measure balance of circit, and actuate servos.
+*/
+
#include "mbed.h"
+#include "string.h"
+#include "MPU6050.h"
+
#include <stdio.h>
#include <errno.h>
-#include "MPU6050.h"
-#include "HMC5883L.h"
-#include "Vector.h"
-#include "Matrix.h"
-#include "Vector_Matrix_operator.h"
-
-// Block devices
-//#include "SPIFBlockDevice.h"
-//#include "DataFlashBlockDevice.h"
#include "SDBlockDevice.h"
-//#include "HeapBlockDevice.h"
-
-// File systems
-
-//#include "LittleFileSystem.h"
#include "FATFileSystem.h"
+#include "myConstants.h"
+
+#define READBUFFERSIZE 70
+#define DELIMITER ","
#define M_PI 3.141592
#define PI2 (2*M_PI)
-/*地磁気センサの補正値（足し合わせる）*/
-#define MAG_FIX_X 338
-#define MAG_FIX_Y 20
-#define MAG_FIX_Z -50
-
/*ジャイロセンサの補正値（引く）*/
#define GYRO_FIX_X 290.5498
#define GYRO_FIX_Y 99.29363
#define GYRO_FIX_Z 61.41011
-// File system declaration
-FATFileSystem fileSystem("fs");
+/*---------ピン指定-------------------*/
+DigitalOut myled(LED1);
+
+DigitalIn switch_off(p30);//sdカードへの書き込みを終了させる
+
+InterruptIn button(p5);//GPSモジュールからの1pps信号を読み取る（立ち下がり割り込み）
-/*-----割り込み--------*/
-Ticker flipper;
-/*--------------------*/
-/*-----タイマー---------*/
-Timer passed_time;
-/*---------------------*/
-/*-------ピン指定------------*/
+InterruptIn input_from_Aileron_1(p11);
+InterruptIn input_from_elevator(p12);//
+InterruptIn input_from_throttle(p13);//
+InterruptIn input_from_rudder(p14);//
+InterruptIn input_from_gear(p15);//
+InterruptIn input_from_Aileron_2(p16);//
+InterruptIn input_from_pitch(p17);//
+InterruptIn input_from_AUX5(p18);//
+
+
+MPU6050 mpu(p28, p27);//sda scl
+
+Serial pc(USBTX, USBRX); // tx, rx
+Serial gps(p9,p10);// gpsとの接続 tx, rx
+SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs
+
//declare PWM pins here
PwmOut ESC (p21);
PwmOut Elevator_servo(p22);
PwmOut Rudder_servo(p23);
+PwmOut Aileron_R_servo(p25);
+PwmOut Aileron_L_servo(p26);
+
+/*-----------------------------------*/
+
+/*-----ファイルポインタ-----*/
+// File system declaration
+FATFileSystem fileSystem("fs");
+/*-----------------------*/
+
+/*------------------------GPSの変数------------------------*/
+int GPS_flag_update=0;
+int GPS_interruptIn=0;
+
+float longtude_gps,latitude_gps,azimuth_gps,speed_gps;
+
+//char NMEA_sentense[READBUFFERSIZE]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
+char NMEA_sentense[READBUFFERSIZE];
+
+/*--------------------------------------------------------*/
-SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs
-MPU6050 mpu(p28, p27);//sda scl
-HMC5883L compass(p28, p27);//sda scl
-
-RawSerial gps(p9,p10); //serial port for gps_module
-RawSerial pc(USBTX, USBRX);
-
-/*--------------------------*/
-
-DigitalIn switch_off(p30);
-DigitalOut led2(LED2);
-
-/*-----------グローバル変数-----------*/
-double Euler_angle[3];
-double Euler_angle_Initiated[3];
-
-float g_hokui,g_tokei;
-int fp_count=0;
-int gps_flag=0;
-int gps_flag_conversion=0;
-
-char gps_data[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10箱を用意している
-char gps_data_2[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10の箱を用意している
-
-unsigned int tokei_int_receive;
-unsigned int hokui_int_receive;
-
-float tokei_float_receive;
-float hokui_float_receive;
-
-float a[3];//加速度の値
-float g[3];//ジャイロの値[rad/s]
-float dpsX,dpsY,dpsZ;
-float AccX,AccY,AccZ;
-
-int sensor_array[6];
-int16_t mag[3];
-
-double Roll_Acc,Pitch_Acc;
-double Yaw;
-double Azi_mag;//地磁気からの方位
-double Quaternion_from_acc[4];//加速度と地磁気からのQuaternion
-
-/*----------------------------------*/
-/*--------------------------行列、ベクトル-----------------------------*/
-
-Vector Quaternion_atitude_from_omega(4);
-Matrix Omega_matrix(4,4),Half_matrix(4,4);
-Matrix Complement_matrix_1(4,4),Complement_matrix_2(4,4);
-
-/*-------------------------------------------------------------------*/
+/*--------------------------角度とセンサ--------------------------*/
+float sensor_array[6];
+float Acc_Pitch_Roll[2];
+float Gyro_Pitch_Roll[2];//pitch and roll
+float omega_z;
+/*--------------------------------------------------------------*/
/*----------PID コントロール-----------------*/
#define Gain_Kp 0.8377
@@ -116,292 +96,81 @@
#define UPPER_DUTY 0.0020
#define LOWER_DUTY 0.0010
+#define THRESHOLD 0.0018
+
double pitch_duty,roll_duty,yaw_duty;
+double first_period,second_period,third_period,fourth_period;
+double fifth_period,sixth_period,seventh_period,eighth_period;
+
+double first_period_old=0.0,second_period_old=0.0,third_period_old=0.0,fourth_period_old=0.0;
+double fifth_period_old=0.0,sixth_period_old=0.0,seventh_period_old=0.0,eighth_period_old=0.0;
+
+double throttle_duty,elevator_duty,rudder_duty,switch_duty,aileron_duty;
+
/*------------------------------------------*/
-
-/*プロトタイプ宣言*/
-void toString_V(Vector& v); // ベクトルデバッグ用
-void gps_rx();
-void gps_convertion();
-/*--------------*/
-
-void toString_V(Vector& v)
-{
-
- for(int i=0; i<v.GetDim(); i++) {
- pc.printf("%.6f\t", v.GetComp(i+1));
- }
- pc.printf("\r\n");
-
-}
-
-void scan_update(int box_sensor[6],int16_t m[3]){
-
- int a[3];//加速度の値
- int g[3];//角速度の値
+/*-----タイマー---------*/
+Timer passed_time;//経過時間の計算
+Timer ch_time;//
- int16_t raw_compass[3];//地磁気センサの値
-
- mpu.setAcceleroRange(0);//acceleration range is +-4G
- mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
-
- mpu.getAcceleroRaw(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis;
- mpu.getGyroRaw(g); // 角速度を格納する配列[rad/s]
- compass.getXYZ(raw_compass);//地磁気の値を格納する配列
-
- box_sensor[0]=-g[0];
- box_sensor[1]=g[1];
- box_sensor[2]=-g[2];
-
- box_sensor[3]=a[0];
- box_sensor[4]=-a[1];
- box_sensor[5]=a[2];
-
- m[0]=(raw_compass[0]);//x
- m[1]=(raw_compass[2]);//y
- m[2]=(raw_compass[1]);//z
-
-}
+float Time_new,Time_old=0.0;
+float Servo_command=0.002;
+/*---------------------*/
+/*---------PWM割り込みフラグ-------*/
+int PWM_flag_update=0;
+/*-------------------------------*/
-void gps_rx(){
-
- if((gps.getc()=='$')&&(gps_flag==0)){
- for(int i=0;i<9;i++){
- gps_data[i]=gps.getc();
- //pc.putc(gps_data[i]);
- }
-
- gps_data[9]='\0';
-
- for(int i=0;i<9;i++){
- gps_data_2[i]=gps.getc();
- //pc.putc(gps_data[i]);
- }
-
- gps_data_2[9]='\0';
-
- }//if(twe.getc()==':')
-
- //pc.printf("%s,%s\r\n",gps_data,gps_data_2);
-
- gps_flag=1;
-
- }//gps_rx ends
+/*----------プロトタイプ宣言----------*/
+void gps_rx();
+void gps_read();
-void gps_convertion(){
- while(1){
- if(gps_flag==1){
- tokei_int_receive=atoi(gps_data);
- hokui_int_receive=atoi(gps_data_2);
-
- //pc.printf("%d,%d\r\n",tokei_int_receive,hokui_int_receive);
-
- tokei_float_receive=float(tokei_int_receive)/pow(10.0,6.0)-100.0;
- hokui_float_receive=float(hokui_int_receive)/pow(10.0,6.0)-100.0;
-
- gps_flag_conversion=1;
-
- }else{gps_flag_conversion=0;}
- }//while ends
-
- }//ends
+double Degree_to_PWM_duty(double degree);
+double PID_duty_pitch(double target,double vol,float dt);
+double PID_duty_rudder(double target,double vol,float dt,float yaw_rate);
+double PID_duty_roll(double target,double vol,float dt);
-void Acc_to_Quaternion(int x_acc,int y_acc,int z_acc,double Yaw,double qua[4]){
-
- double Roll_before = double(y_acc)/double(z_acc);
- double Roll = atan(Roll_before);
-
- double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
- double Pitch = -atan(Pitch_before);
-
- //Yaw=0.0;
-
- /*Quaternionの要素へオイラー角を変換する*/
- qua[0]=cos(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
- qua[1]=sin(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)-cos(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
- qua[2]=cos(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0);
- qua[3]=cos(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0)-sin(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0);
-
-
- }
-
-void Acc_to_Pitch_Roll(int x_acc,int y_acc,int z_acc,double qua[2]){
-
- double Roll_before = double(y_acc)/double(z_acc);
- double Roll = atan(Roll_before);
-
- double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
- double Pitch = -atan(Pitch_before);
+void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]);
+void scan_update(float box_sensor[6]);
+void sensor_update();
-
- qua[0]=Pitch;
- qua[1]=Roll;
-
- }
+//gpsの1pps信号の割り込み
+void flip();
-void Correct_n(int x_acc,int y_acc,int z_acc,double n[4]){
-
- //y_acc*=-1.0;
-
- n[0]=-double(y_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
- n[1]=double(x_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
- n[2]=0.0;
-
- n[3]=sqrt(double(x_acc*x_acc+y_acc*y_acc))/double(z_acc);
-
- }
-double Geomagnetism_correction(int x_acc,int y_acc,int z_acc,int16_t magnet[3]){
-
- Vector Mag_from_sensor(3),Mag_fixed_sensor(3); //座標系
- Matrix Rotation(3, 3); //検算の行列
+void Initialize_ESC();
+void Activate_ESC();
- double Correct_vector_n[4];//回転行列の成分
- Correct_n(x_acc,y_acc,z_acc, Correct_vector_n);
-
- double n_x=Correct_vector_n[0];
- double n_y=Correct_vector_n[1];
- double n_z=Correct_vector_n[2];
- double theta=-Correct_vector_n[3];
-
- float Rotate_element[9] = {
- n_x*n_x*(1-cos(theta))+cos(theta) , n_x*n_y*(1-cos(theta)) , n_y*sin(theta),
- n_x*n_y*(1-cos(theta)) ,n_y*n_y*(1-cos(theta))+cos(theta) , -n_x*sin(theta),
- n_y*sin(theta) , n_x*sin(theta) , cos(theta)
- };
-
- Rotation.SetComps(Rotate_element);
- //x y z
- float Geomagnetism[3]={ float(magnet[1]+MAG_FIX_X)
- , float(magnet[0]+MAG_FIX_Y)
- , float( (-1.0)*(magnet[2]+MAG_FIX_Z ))};
-
- Mag_from_sensor.SetComps(Geomagnetism);
-
- Mag_fixed_sensor=Rotation*Mag_from_sensor;//地磁気センサの値を加速度センサによる傾きで補正。
-
- float Mag_fixed_x= Mag_fixed_sensor.GetComp(1);
- float Mag_fixed_y= Mag_fixed_sensor.GetComp(2);
-
- double Azi=atan2(double(Mag_fixed_y),double(Mag_fixed_x));
-
- /*
- if(Azi < 0.0) // fix sign
- Azi += PI2;
-
- if(Azi > PI2) // fix overflow
- Azi -= PI2;
- */
-
- return Azi;
-
- }
+void Input_Aileron_1();
+void Input_elevator();
+void Input_throttle();
+void Input_rudder();
+void Input_gear();
+void Input_Aileron_2();
+void Input_pitch();
+void Input_AUX5();
-//void led2_thread(void const *argument) {
-void imu_thread() {
- while (true) {
- }//while ends
-}
-
-void Quaternion_to_Euler(float q0,float q1,float q2,float q3,double Euler[3]){
-
- //float pitch,roll,yaw;
-
- Euler[0]=RAD_TO_DEG*atan2(double( 2*(q2*q3+q0*q1)), double (q0*q0-q1*q1-q2*q2+q3*q3) );
- Euler[1]=-RAD_TO_DEG*asin(double(2*(q1*q3-q0*q2)));
- Euler[2]=RAD_TO_DEG*atan2(double(2*(q1*q2+q0*q3)), double(q0*q0+q1*q1-q2*q2-q3*q3));
+void Input_Aileron_1_fall();
+void Input_elevator_fall();
+void Input_throttle_fall();
+void Input_rudder_fall();
+void Input_gear_fall();
+void Input_Aileron_2_fall();
+void Input_pitch_fall();
+void Input_AUX5_fall();
+/*---------------------------------*/
- }
-
-double Degree_to_PWM_duty(double degree){
-
- double duty=0.0;
-
- duty=(0.2/1000)/20.0*degree;
-
- if((duty+NUTRAL)>=UPPER_DUTY){
- duty=UPPER_DUTY-NUTRAL;
- }else if((duty+NUTRAL)<=LOWER_DUTY){
- duty=NUTRAL-LOWER_DUTY;
- }else{}
-
- return duty;
+int main() {
- }
-
-double PID_duty(double target,double vol,float dt){
- //ここで角度の単位で偏差は入力される
- //出力はPWMで
- double duty=0.0;
- double add_duty=0.0;
-
- Prop_p=target-vol;
-
- pc.printf("%f/r/n",Prop_p);
-
- Int_p+=Prop_p*double(dt);
- Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
-
- Pre_Prop_p=Prop_p;
-
- //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
-
- add_duty=1.0*Prop_p;
-
- duty+=add_duty;
- duty=Degree_to_PWM_duty(duty);
-
- return duty;
-
-}
-
-void Initialize_ESC(){
-
- ESC.pulsewidth(0.002);
wait(3);
- ESC.pulsewidth(0.001);
- wait(5);
- }
+ /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/
-void Activate_ESC(){
- ESC.pulsewidth(0.001);
- wait(5);
-
- }
-
-// Entry point for the example
-int main()
-{
- gps.baud(115200);//GT-720Fボーレート
- pc.baud(115200);
- //imu_thread
-
- compass.init();//hmc5883の起動
-
- Thread thread1 (gps_convertion);
- gps.attach(gps_rx, Serial::RxIrq);
-
- /*define period of serve and ESC*/
- ESC.period(SERVO_PERIOD);
- Elevator_servo.period(SERVO_PERIOD);
- Rudder_servo.period(SERVO_PERIOD);
- /*------------------------------*/
- Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
- Rudder_servo.pulsewidth(NUTRAL);
-
- Activate_ESC();
-
- float Time_old=0.0;//時間初期化
- passed_time.start();//タイマー開始
-
- pc.printf("--- Mbed OS filesystem example ---\n");
+ pc.printf("--- Mbed OS filesystem example ---\n");
// Try to mount the filesystem
pc.printf("Mounting the filesystem... ");
@@ -422,215 +191,539 @@
}
// Open the numbers file
- pc.printf("Opening \"/fs/numbers.txt\"... ");
+ pc.printf("Opening \"/fs/log.csv\"... ");
fflush(stdout);
- FILE* f = fopen("/fs/numbers.txt", "r+");
+ FILE* f = fopen("/fs/log.csv", "r+");
+ FILE* fp = fopen("/fs/log.txt", "r+");
+
pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
if (!f) {
// Create the numbers file if it doesn't exist
pc.printf("No file found, creating a new file... ");
fflush(stdout);
- f = fopen("/fs/numbers.txt", "w+");
+ f = fopen("/fs/log.csv", "w+");
+ fp = fopen("/fs/log.txt", "w+");
+
pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
}
-
- /*初期姿勢のQuaternionの設定*/
- scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z
- Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
-
- Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
+
- float Initialize_atitude[4];
- Initialize_atitude[0]=Quaternion_from_acc[0];
- Initialize_atitude[1]=Quaternion_from_acc[1];
- Initialize_atitude[2]=Quaternion_from_acc[2];
- Initialize_atitude[3]=Quaternion_from_acc[3];
-
- Quaternion_atitude_from_omega.SetComps(Initialize_atitude);//ジャイロで計算するQuaternionの初期化
- while(1){
-
- /*gpsから来た文字列の処理
- if((gps_flag==1)&&(gps_flag_conversion==1)){
-
- err = fprintf(f,"%f,%f\r\n",tokei_float_receive,hokui_float_receive);
-
- gps_flag=0;
- gps_flag_conversion=0;
-
- }else{}
- */
-
- scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z
- float Time_new=float(passed_time.read());//時間の取得
-
- //地磁気の補正用に使う。
- //pc.printf("%d,%d,%d\r\n",mag[1],mag[0],mag[2]);
-
- //ジャイロの補正用に使う
- //pc.printf("%d,%d,%d\r\n",sensor_array[0],sensor_array[1],sensor_array[2]);
+ /*define period of serve and ESC*/
+ ESC.period(SERVO_PERIOD);
+ Elevator_servo.period(SERVO_PERIOD);
+ Rudder_servo.period(SERVO_PERIOD);
+ Aileron_R_servo.period(SERVO_PERIOD);
+ Aileron_L_servo.period(SERVO_PERIOD);
+ /*------------------------------*/
+ Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
+ Rudder_servo.pulsewidth(NUTRAL);
+ Aileron_R_servo.pulsewidth(NUTRAL);
+ Aileron_L_servo.pulsewidth(NUTRAL);
+
+ Activate_ESC();//Activate ESC
+
+ /*--------------ピン変化割り込みに対応した関数の宣言--------------*/
+ button.fall(&flip);//GPSモジュールからの1pps信号を立ち下がり割り込みで読み取る。
+
+ input_from_throttle.rise(&Input_throttle);//
+
+ input_from_Aileron_1.fall(&Input_Aileron_1_fall);
+ input_from_elevator.fall(&Input_elevator_fall);//
+ input_from_throttle.fall(&Input_throttle_fall);//
+ input_from_rudder.fall(&Input_rudder_fall);//
+ input_from_gear.fall(&Input_gear_fall);
+ input_from_Aileron_2.fall(&Input_Aileron_2_fall);//
+ input_from_pitch.fall(&Input_pitch_fall);//
+ input_from_AUX5.fall(&Input_AUX5_fall);//
+
+ ch_time.start();//時間計測スタート
+ /*----------------------------------------------------------*/
+
+ pc.baud(115200);
+ gps.baud(115200);
+
+ //gps.attach(gps_rx, Serial::RxIrq);//シリアル割り込み
+ //pc.attach(gps_rx, Serial::RxIrq);//シリアル割り込み
+
+ passed_time.start();//タイマー開始
+ Time_old=float(passed_time.read());
+
+ /*初期姿勢角の算出*/
+ scan_update(sensor_array);//角速度、加速度を求める
+ Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll);
+ Gyro_Pitch_Roll[0]=Acc_Pitch_Roll[0];//初期姿勢角の算出 Pitch
+ Gyro_Pitch_Roll[1]=Acc_Pitch_Roll[1];//初期姿勢角の算出 Roll
+
+ pc.printf("Start!!\r\n");
+
+ while(1) {
+
+ switch (GPS_interruptIn) {
+ case 1:
+
+ gps_rx();
+ gps_read();
+ GPS_interruptIn--;
+ break;
- /*----------------ジャイロセンサからQuaternionを求める----------------*/
- float omega_x=float(( float(sensor_array[0])-GYRO_FIX_X )/ 7505.7);
- float omega_y=float(( float(sensor_array[1])-GYRO_FIX_Y )/ 7505.7);
- float omega_z=float(( float(sensor_array[2])-GYRO_FIX_Z )/ 7505.7);
-
- float omega[16] = {
+ default:
- 0.0 , -omega_x , -omega_y, -omega_z,
- omega_x ,0.0 ,omega_z ,-omega_y ,
- omega_y , -omega_z , 0.0 , omega_x ,
- omega_z , omega_y ,-omega_x , 0.0
-
- };
-
- Omega_matrix.SetComps(omega);
+ sensor_update();
+ break;
+ }//switch end
+
+ /*
+ ここでプロポの信号のパルス幅を読み取って、手動or 自動の切り替えを行う必要がある。
+ 以下のpitch control等は自動操縦とみなしてよい。
+ */
+
+
+ if(switch_duty>THRESHOLD){//オートパイロット
- float half[16] ={
- (Time_new-Time_old)*0.5,0.0,0.0,0.0,
- 0.0,(Time_new-Time_old)*0.5,0.0,0.0,
- 0.0,0.0,(Time_new-Time_old)*0.5,0.0,
- 0.0,0.0,0.0,(Time_new-Time_old)*0.5
- };
-
+ //Pitch control
+ //pitch_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old));
+ elevator_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old));
+ Elevator_servo.pulsewidth(elevator_duty);
- Half_matrix.SetComps(half);
- //Quaternion_atitude_from_omega+=(Time_new-Time_old)*
- Quaternion_atitude_from_omega +=Half_matrix*Omega_matrix*Quaternion_atitude_from_omega;
+ //roll control
+ //roll_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old))
+ aileron_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old));
- Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize();
+ //角速度だけフィードバック
+ rudder_duty=NUTRAL+PID_duty_rudder(0.0,0.0,(Time_new-Time_old),omega_z);
+ ///pc.printf("%f\r\n",rudder_duty);
- //Time_old=Time_new;//時間の更新
-
- /*
- pc.printf("%f,%f,%f,%f\r\n"
- ,Quaternion_atitude_from_omega.GetComp(1),Quaternion_atitude_from_omega.GetComp(2)
- ,Quaternion_atitude_from_omega.GetComp(3),Quaternion_atitude_from_omega.GetComp(4));
- */
-
- /*----------------------------------------------------------------*/
-
- /*
- err = fprintf(f,"A,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n"
- ,val,sensor_array[0],sensor_array[1],sensor_array[2],sensor_array[3],sensor_array[4],sensor_array[5]
- ,sensor_array[6],sensor_array[7],sensor_array[8]);
- */
-
- /*--------------------加速センサで地磁気の値を補正する --------------------*/
-
- if(9.8065*1.1 >
- sqrt(double(sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5]))
- / 16384.0 * 9.81)
- {
+ //スロットルはマニュアル
+ //Servo_command
+ if( (float(passed_time.read())-Servo_command)>0.003 ){
- Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
-
- //pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG);
-
- double pitch_and_roll[2];
- Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll);
-
- //pc.printf("Pitch %f,Roll %f\r\n",pitch_and_roll[0]*RAD_TO_DEG,pitch_and_roll[1]*RAD_TO_DEG);
+ ESC.pulsewidth(throttle_duty);
+ Elevator_servo.pulsewidth(elevator_duty);
+ Rudder_servo.pulsewidth(rudder_duty);
+ Aileron_R_servo.pulsewidth(aileron_duty);
+ Aileron_L_servo.pulsewidth(aileron_duty);
- Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
-
- /*Quaternion_from_acc_fixedの要素をfloatへ変換する*/
- float Quaternion_from_acc_fixed[4];
- Quaternion_from_acc_fixed[0]=float(Quaternion_from_acc[0]);
- Quaternion_from_acc_fixed[1]=float(Quaternion_from_acc[1]);
- Quaternion_from_acc_fixed[2]=float(Quaternion_from_acc[2]);
- Quaternion_from_acc_fixed[3]=float(Quaternion_from_acc[3]);
-
- Vector Qua_acc(4);
-
- Qua_acc.SetComps(Quaternion_from_acc_fixed);
- /*---------------------------------------------*/
-
- /*---------------------相補フィルタのゲインに用いる---------------------*/
- float comp_1[16]={0.95,0.0,0.0,0.0,
- 0.0,0.95,0.0,0.0,
- 0.0,0.0,0.95,0.0,
- 0.0,0.0,0.0,0.95
- };
- /*--------------------------------------------------------------------*/
- /*---------------------quaternionの時間微分に用いる---------------------*/
- float comp_2[16]={0.05,0.0,0.0,0.0,
- 0.0,0.05,0.0,0.0,
- 0.0,0.0,0.05,0.0,
- 0.0,0.0,0.0,0.05
- };
-
- Complement_matrix_1.SetComps(comp_1);
- Complement_matrix_2.SetComps(comp_2);
-
- /*--------------------------------------------------------------------*/
+ Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新
+ }else{}
+
+ }else{//マニュアルモード
+
+ if( (float(passed_time.read())-Servo_command)>0.003 ){
+
+ ESC.pulsewidth(throttle_duty);
+ Elevator_servo.pulsewidth(elevator_duty);
+ Rudder_servo.pulsewidth(rudder_duty);
+ Aileron_R_servo.pulsewidth(aileron_duty);
+ Aileron_L_servo.pulsewidth(aileron_duty);
+
+ Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新
+
+ }else{}
-
- /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/
-
- Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega
- +Complement_matrix_2*Qua_acc;
-
- /*----------------------------------------------------------------------------*/
-
-
- /*
- pc.printf("%f,%f,%f,%f,%f,%f,%f,%f\r\n"
- ,Quaternion_from_acc[0],Quaternion_atitude_from_omega.GetComp(1)
- ,Quaternion_from_acc[1],Quaternion_atitude_from_omega.GetComp(2)
- ,Quaternion_from_acc[2],Quaternion_atitude_from_omega.GetComp(3)
- ,Quaternion_from_acc[3],Quaternion_atitude_from_omega.GetComp(4));
- */
-
- }else{}
+ }
+
+ 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);
+ err = fprintf(f,"%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps);
+ err = fprintf(fp,"%f,%f\r\n",latitude_gps,longtude_gps);
+
+ Time_old=Time_new;//時間更新
+
+ if(switch_off==1){
+ //fclose(fp);
+ //flipper.detach();
+
+ err = fclose(f);
+ err = fclose(fp);
+
+ pc.printf("Writing finish!");
+ myled=0;
+ break;
+
+ }
+
+ //wait(0.002);//この値は10倍されると考える
+ }//while ends
+
+}//main ends
+
+void gps_rx(){
+
+ // __disable_irq(); // 割り込み禁止
+
+ int i=0;
+
+ while(gps.getc()!='$'){
+ }
+ while( (NMEA_sentense[i]=gps.getc()) != '\r'){
+ //while( (NMEA_sentense[i]=pc.getc()) != '\r'){
+ //pc.putc(NMEA_sentense[i]);
+ i++;
+ if(i==READBUFFERSIZE){
+ i=(READBUFFERSIZE-1);
+ break;
+ }
+ }
+ NMEA_sentense[i]='\0';
+ //pc.printf("\r\n");
+
+ if(GPS_flag_update==0){
+ GPS_flag_update++;
+ }
+
+ //__enable_irq(); // 割り込み許可
+
+}//void gps_rx
+
+void gps_read(){
+ /*------gps のNMEAフォーマットの処理プログラム----------------*/
+ float temp=0.0, deg=0.0, min=0.0;
+ char *pszTime;//UTC時刻
+ char* pszLat;////緯度
+ char* pszLong;//経度
+ char* pszSp;//速度
+ char* pszSpd;//速度方向
+ char* pszDate;//日付
+
+ //strtokで処理した文字列は内容がNullになるので、テストでは、毎回NMEA_sentense[128]を初期化しないといけない
+ //char NMEA_sentense[128]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
+
+ //Time_old=float(passed_time.read());
+
+
+
+ if((GPS_flag_update==1)&&(0==strncmp( "GPRMC", NMEA_sentense, 5 ))){//GPS_flag_updateも判定に加えた方がいい
+ //if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){//GPS_flag_updateも判定に加えた方がいい
+
+ strtok( NMEA_sentense, DELIMITER ); // $GPRMC
+ pszTime = strtok( NULL, DELIMITER ); // UTC時刻
+ strtok( NULL, DELIMITER ); // ステータス
+ pszLat = strtok( NULL, DELIMITER ); // 緯度(dddmm.mmmm)
+ strtok( NULL, DELIMITER ); // 北緯か南緯か
+ pszLong = strtok( NULL, DELIMITER ); // 経度(dddmm.mmmm)
+ strtok( NULL, DELIMITER ); // 東経か西経か
+
+ pszSp = strtok( NULL, DELIMITER ); // 速度（vvv.v）
+ pszSpd = strtok( NULL, DELIMITER ); // 速度方向（ddd.d）
+ pszDate = strtok( NULL, DELIMITER ); // 日付
+
+ if( NULL != pszLong ){//pszLongがnullでないのならば
+ temp = atof(pszLat);
+ deg = (int)(temp/100);
+ min = temp - deg * 100;
+ latitude_gps = deg + min / 60;//latitudeの算出
+
+ temp = atof(pszLong);
+ deg = (int)(temp/100);
+ min = temp - deg * 100;
+ longtude_gps = deg + min / 60;//longtudeの算出
+
+ speed_gps=atof(pszSp)*KNOT_TO_METER_PER_SEC;
+
+ azimuth_gps=atof(pszSpd);
+
+ GPS_flag_update--;
+
+ //pc.printf("%f,%f\r\n",longtude_gps,latitude_gps);
+
+ }else{}//end of ( NULL != pszLong )
+
+ }else{}//end of if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){
+ }
+
+
+void sensor_update(){
+ scan_update(sensor_array);//角速度、加速度を求める
+
+ Time_new=float(passed_time.read());//時間の更新
+
+ Gyro_Pitch_Roll[0]+=sensor_array[1]*(Time_new-Time_old);//初期姿勢角の算出 Pitch
+ Gyro_Pitch_Roll[1]+=sensor_array[0]*(Time_new-Time_old);//初期姿勢角の算出 Roll
+
+
+ /*--------------------加速センサでジャイロの値を補正する --------------------*/
+
+
+
+ if(9.8065*1.05 > sqrt((sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5])))
+ {
+ Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll);
+ Gyro_Pitch_Roll[0]*=0.95;//
+ Gyro_Pitch_Roll[1]*=0.95;//
+ Gyro_Pitch_Roll[0]+=(0.05*Acc_Pitch_Roll[0]);//
+ Gyro_Pitch_Roll[1]+=(0.05*Acc_Pitch_Roll[1]);//
+
+
+ }else{}
+
+ //pc.printf("%f,%f,%f\r\n",Time_new,Acc_Pitch_Roll[0]*RAD_TO_DEG,Acc_Pitch_Roll[1]*RAD_TO_DEG);
+ //pc.printf("%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1]);
+
+ //Time_old=Time_new;
+
+ }
+double Degree_to_PWM_duty(double degree){
+
+ double duty=0.0;
+
+ duty=(0.2/1000)/20.0*degree;
+
+ if((duty+NUTRAL)>=UPPER_DUTY){
+ duty=UPPER_DUTY-NUTRAL;
+ }else if((duty+NUTRAL)<=LOWER_DUTY){
+ duty=NUTRAL-LOWER_DUTY;
+ }else{}
+
+ return duty;
+
+ }
+
+double PID_duty_pitch(double target,double vol,float dt){
+ //ここで角度の単位で偏差は入力される
+ //出力はPWMで
+ double duty=0.0;
+ double add_duty=0.0;
+
+ Prop_p=target-vol;
+
+ //pc.printf("%f/r/n",Prop_p);
+
+ Int_p+=Prop_p*double(dt);
+ Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
+
+ Pre_Prop_p=Prop_p;
+
+ //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
+
+ add_duty=1.0*Prop_p;
+
+ duty+=add_duty;
+ duty=Degree_to_PWM_duty(duty);
+
+ return duty;
+
+}
+
+double PID_duty_rudder(double target,double vol,float dt,float yaw_rate){
+ //ここで角度の単位で偏差は入力される
+ //出力はPWMで
+ double duty=0.0;
+ double add_duty=0.0;
+
+ Prop_p=target-vol;
+
+ //pc.printf("%f/r/n",Prop_p);
+
+ Int_p+=Prop_p*double(dt);
+ Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
+
+ Pre_Prop_p=Prop_p;
+
+ //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
- /*
- pc.printf("%f,%f,%f,%f,%f\r\n"
- ,Time_new
- ,Quaternion_atitude_from_omega.GetComp(1)
- ,Quaternion_atitude_from_omega.GetComp(2)
- ,Quaternion_atitude_from_omega.GetComp(3)
- ,Quaternion_atitude_from_omega.GetComp(4));
- */
- /*-----------Quaternionからオイラー角への変換-----------*/
- Quaternion_to_Euler(Quaternion_atitude_from_omega.GetComp(1)
- ,Quaternion_atitude_from_omega.GetComp(2)
- ,Quaternion_atitude_from_omega.GetComp(3)
- ,Quaternion_atitude_from_omega.GetComp(4)
- ,Euler_angle);
-
- /*-----------オイラー角の表示----------------------*/
+ //add_duty=1.0*Prop_p;
+ //今回は角速度に反応させてラダーを動かす
+ add_duty=double(yaw_rate)*0.1;
+
+ duty+=add_duty;
+ duty=Degree_to_PWM_duty(duty);
+
+ return duty;
+
+}
+
+double PID_duty_roll(double target,double vol,float dt){
+ //ここで角度の単位で偏差は入力される
+ //出力はPWMで
+ double duty=0.0;
+ double add_duty=0.0;
+
+ Prop_p=target-vol;
+
+ //pc.printf("%f/r/n",Prop_p);
+
+ Int_p+=Prop_p*double(dt);
+ Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
+
+ Pre_Prop_p=Prop_p;
+
+ //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
+
+ add_duty=1.0*Prop_p;
+
+ duty+=add_duty;
+ duty=Degree_to_PWM_duty(duty);
+
+ return duty;
- pc.printf("%f,%f,%f,%f,%f\r\n"
- ,Time_new
- ,Euler_angle[0]
- ,Euler_angle[1]
- ,Euler_angle[2]
- );
-
- /*--------------------------------------------------*/
- //Pitch control
- pitch_duty=NUTRAL+PID_duty(0.0,Euler_angle[1],(Time_new-Time_old));
- Elevator_servo.pulsewidth(pitch_duty);
-
+}
+void Initialize_ESC(){
+
+ ESC.pulsewidth(0.002);
+ wait(3);
+
+ ESC.pulsewidth(0.001);
+ wait(5);
+
+ }
+
+
+void Activate_ESC(){
+
+ ESC.pulsewidth(0.001);
+ wait(5);
+
+ }
+
+void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]){
+
+ float Roll_before = (y_acc)/(z_acc);
+ float Roll = atan(Roll_before);
- Time_old=Time_new;//時間の更新
-
- wait(0.001);
-
- /*----------------------------------------------------------------*/
+ float Pitch_before = (x_acc/sqrt((y_acc*y_acc+z_acc*z_acc)) );
+ float Pitch = -atan(Pitch_before);
+
+ qua[0]=Pitch*RAD_TO_DEG;
+ qua[1]=Roll*RAD_TO_DEG;
+ //float Acc_Pitch_Roll[2];
+
+ }
+
+void scan_update(float box_sensor[6]){
+
+ float a[3];//加速度の値
+ float g[3];//ジャイロの値[rad/s]
+
+ //int16_t raw_compass[3];//地磁気センサの値
+
+ mpu.setAcceleroRange(0);//acceleration range is +-4G
+ mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
+
+ mpu.getAccelero(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis;
+ mpu.getGyro(g);// 角速度を格納する配列[rad/s]
+
+ //pc.printf("%f,%f,%f\r\n",g[0],g[1],g[2]);
+
+ //compass.getXYZ(raw_compass);//地磁気の値を格納する配列
+
+ box_sensor[0]=((-g[0])-(GYRO_FIX_X/7505.7))*RAD_TO_DEG;
+ box_sensor[1]=((g[1])-(GYRO_FIX_Y/7505.7))*RAD_TO_DEG;
+ box_sensor[2]=((-g[2])-(GYRO_FIX_Z/7505.7))*RAD_TO_DEG;
+
+ omega_z=box_sensor[2];
+
+ box_sensor[3]=a[0];
+ box_sensor[4]=-a[1];
+ box_sensor[5]=a[2];
+
+ //pc.printf("%f,%f,%f\r\n",box_sensor[0],box_sensor[1],box_sensor[2]);
+}
+
+void flip(){
+
+ GPS_interruptIn++;
+
+};
+
+
+//ch1
+void Input_Aileron_1(){
+
+ first_period_old=ch_time.read();
+ //pc.printf("ch1=%f",second_period);
+ };
+
+void Input_Aileron_1_fall(){
+ first_period=ch_time.read();
+ aileron_duty=first_period-first_period_old;
+};
+
+
+//ch2
+void Input_elevator(){
+ second_period_old=ch_time.read();
+};
- if(switch_off==1){
- // Close the file which also flushes any cached writes
- pc.printf("Closing \"/fs/numbers.txt\"... ");
- err = fclose(f);
- break;
- }
-
- }//while(1) ends
-}//main ends
+void Input_elevator_fall(){
+ second_period=ch_time.read();
+ elevator_duty=second_period-second_period_old;
+ };
+
+
+//ch3
+void Input_throttle(){
+
+ first_period_old=ch_time.read();
+ second_period_old=ch_time.read();
+ third_period_old=ch_time.read();
+ fourth_period_old=ch_time.read();
+ fifth_period_old=ch_time.read();
+ sixth_period_old=ch_time.read();
+
+
+}
+
+void Input_throttle_fall(){
+ third_period=ch_time.read();
+
+ /*デコード*/
+ throttle_duty=third_period-third_period_old;
+ //rudder_duty=fourth_period-fourth_period_old;
+ //elevator_duty=second_period-second_period_old;
+ //aileron_duty=first_period-first_period_old;
+
+ //pc.printf("throttle_duty=%f:rudder_duty=%f:elevator_duty=%f\r\n",throttle_duty,rudder_duty,elevator_duty);
+}
+
+//ch4
+void Input_rudder(){
+ fourth_period_old=ch_time.read();
+}
+
+void Input_rudder_fall(){
+ fourth_period=ch_time.read();
+ rudder_duty=fourth_period-fourth_period_old;
+}
+
+//ch5
+void Input_gear(){
+ fifth_period_old=ch_time.read();
+
+}
+
+void Input_gear_fall(){
+ fifth_period=ch_time.read();
+ switch_duty=fifth_period-fifth_period_old;
+}
+
+//ch6
+void Input_Aileron_2(){
+ sixth_period_old=ch_time.read();
+ }
+
+void Input_Aileron_2_fall(){
+ sixth_period=ch_time.read();
+ };
+
+//ch7
+void Input_pitch(){
+ seventh_period_old=ch_time.read();
+}
+
+void Input_pitch_fall(){
+ seventh_period=ch_time.read();
+ }
+
+//ch8
+void Input_AUX5(){
+ eighth_period_old=ch_time.read();
+}
+
+void Input_AUX5_fall(){
+ eighth_period=ch_time.read();
+}