yal kaiyo
/
130111_YNU_MPU_4
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
130111_YNU_MPU_3
by 
yal kaiyo
main.cpp
- Committer:
- yal_kaiyo
- Date:
- 2013-01-14
- Revision:
- 15:571b34671309
- Parent:
- 14:feea7e2a51b8
File content as of revision 15:571b34671309:
#include "mbed.h"
#include "SDFileSystem.h"
#define pi 3.1416
#define data 6
#define zer 0.0
#define hal 0.5
#define one 1.0
#define dtr 0.01745
#define rtd 57.3
//parameter//
#define dt1 0.2 // 1/N
#define hdt 0.1 // 1/2N
#define md 6 //kizami N
#define mt 7 //md+1
#define mx 4
#define mv 8
#define mh 56 //mt*mv
Serial pc(USBTX, USBRX);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
SDFileSystem sd(p5, p6, p7, p8, "sd");
Serial mavc(p9, p10);
DigitalIn stop_sd(p11);
int main() {
// ******************** TORATANI ********************
pc.baud(921600);
mavc.baud(115200);
int flag_lc = 0;
int flag_cm = 0;
int loop = 100; // for 2 minuts -> loop = 1200
char buf_char;
unsigned int buf_hex_rx[30] = {0};
unsigned int buf_dec_rx[10] = {0};
unsigned int buf_dec_tx[3] = {0};
unsigned int buf_hex_tx[3][4] = {0};
// rx
double acc[3] = {0};
double gyro[3] = {0};
double azi;
double alt;
double GPS[2] = {0};
// tx
float com[3] = {0};
unsigned int H;
unsigned int D[data] = {0};
H = 0x02; // header of dateset2
D[0] = 0x7f; D[1] = 0xff; // p_com
D[2] = 0x7f; D[3] = 0xff; // q_com
D[4] = 0x7f; D[5] = 0xff; // r_com
/*
D[0] = 0x7; D[1] = 0xf; D[2] = 0xf; D[3] = 0xf; // p_com
D[4] = 0x7; D[5] = 0xf; D[6] = 0xf; D[7] = 0xf; // q_com
D[8] = 0x7; D[9] = 0xf; D[10] = 0xf; D[11] = 0xf; // r_com
*/
// ******************** TORATANI ********************
// ******************** HOSHINO *********************
float time = 0; //time
float flag_int= 0;
//initial condition
float drf = 100; //syuutandaunrenji
float ht0 = 0;
float gm0 = 20*pi/180;
float ua = 10; //initial horizontal velosity
//terminal condition
float htf = 5;
float gmf = 5*pi/180;
float xis = 0;
float ets = 0;
float psis = 0;
float x0 = 0;
float y0 = 0;
float h0 = 0;
float b0 = 0;
float b1 = 0;
float b2 = 0;
float b3 = 0;
float htc = 0;
float x = 0; //down range
float xis1 = 0;
float ets1 = 0;
float xir = 0;
float etr = 0;
float drc = 0;
float drp = 0;
float dt = 0.1;
float psr = 0;
float htr = 0;
float avp = 0;
float avq = 0;
float avr = 0;
float s = 0;
float c = 0;
float c0 = 0;
float c1 = 0;
float dr[mt] = {0};
float time1 = 0;
float kp = 0;
float kq = 0;
float kr = 0;
float psc = 0;
float etc = 0;
float xic = 0;
float apc = 0;
//homotopy
//initial condition
float rps0 = 90.0;
float rxi0 = -60.0; //terminal xi
float ret0 = -20.0; //terminal et
float ps0 = rps0*dtr;
float xi0 = rxi0/drf;
float et0 = ret0/drf;
float eps = 0.00000001;
float ueps = 0.000001;
float ier = 0.0;
float sum = 0.0;
float ph0[mh] = {0};
float phi[mh] = {0};
float fn[mh] = {0};
float wk[mh] = {0};
int iwk[mh] = {0};
float dph1[mh] = {0};
float dph2[mh] = {0};
float dph3[mh] = {0};
float dph4[mh] = {0};
float fph[mh][mh] = {0};
float fpi[mh][mh] = {0};
float x1[4][mt] = {0};
float rl[4][mt] = {0};
float qmax=0;
float rdet =1;
float idet =0;
float t = 0.0;
float phf[mh]={0};
float ap[mt]={0};
float ps[mt]={0};
float xi[mt]={0};
float et[mt]={0};
float xi1[mt]={0};
float et1[mt]={0};
//********************* HOSHINO *********************
//********************* OHTSU ***********************
//teigi
float grv = 9.807;
float swg = 0.04695;
float wgt = 0.76;
float rho = 1.2255;
float tt1 = 0.8;
float omg1 = 1.0;
float zet1 = 0.7;
float tt2 = 0.37;
float omg2 = 2.6;
float zet2 = 0.7;
float gmr = 20;
float vmr = 0;
float pse = 0;
float phir = 0;
float ete = 0;
float hte = 0;
float xie = 0;
float hte00 = 0;
float dhdt = 0;
float phic00 = 0;
float dclda = 0.084;
float v2 = 0;
float a22 = 0;
float a23 = 0;
float a32 = 0;
float b21 = 0;
float aaa = 0;
float gkp = 0;
float gkd = 0;
float ck1 = 0;
float ck2 = 0;
float phff = 0;
float phic = 0;
float avpc = 0;
float avqc = 0;
float avrc = 0;
//********************* OHTSU ***********************
FILE *fp = fopen("/sd/sdtest.txt", "w");
if(fp == NULL) {
error("Could not open file for write\n");
}
// **************** waiting @LC or @FB ****************
// * @LC:0x40 0x4C 0x43 0x0D, @FB:0x40 0x46 0x42 0x0D *
// /////////////////////////////////////////////////////
// ////////////////////// NOTICE! //////////////////////
// // flag_lc==1 is debug mode. skip this while loop. //
// ////////// for flight mode, flag_lc is 0! ///////////
// /////////////////////////////////////////////////////
while(flag_lc==1){
buf_char = mavc.getc();
buf_hex_rx[0] = (unsigned int)buf_char;
for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; }
if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x46 && buf_hex_rx[27]==0x42 && buf_hex_rx[26]==0x0D){
// ******************** waiting @LC ********************
// ******************** homotopy ********************
////SYOKIKATEIKAI////
for(int it=0;it<mt;it++){
double tm=dt1*it;
int i0=mv*it;
ph0[i0]=zer;
if(fabs(ps0)<=0.0000000001){
ph0[i0+1]=(ps0+1.0)*tm;
}
else{
ph0[i0+1] = ps0*tm;
}
ph0[i0+2]= xi0*tm;
ph0[i0+3]= et0*tm;
ph0[i0+4]=-hal;
ph0[i0+5]=-hal;
ph0[i0+6]=-hal;
ph0[i0+7]=-hal;
}
for(int it=0;it<mh;it++){
phi[it]=ph0[it];
}
for(int it=0;it<mh;it++){
phf[it]=ph0[it];
}
//rungekutta
for(int ir=0;ir<mt;ir++){
//k1
////SABUNSHIKI//////
for(int it=0;it<mt;it++){
int i0=mv*it;
x1[0][it]=phi[i0+0];
x1[1][it]=phi[i0+1];
x1[2][it]=phi[i0+2];
x1[3][it]=phi[i0+3];
rl[0][it]=phi[i0+4];
rl[1][it]=phi[i0+5];
rl[2][it]=phi[i0+6];
rl[3][it]=phi[i0+7];
}
fn[0]=x1[0][0];
fn[1]=x1[1][0];
fn[2]=x1[2][0];
fn[3]=x1[3][0];
for(int it=0;it<md;it++){
int i0=mx+mv*it;
int it1=it+1;
double bkp = hal*(x1[0][it]+x1[0][it1]);
double bps = hal*(x1[1][it]+x1[1][it1]);
double br1 = hal*(rl[0][it]+rl[0][it1]);
double br2 = hal*(rl[1][it]+rl[1][it1]);
double br3 = hal*(rl[2][it]+rl[2][it1]);
double br4 = hal*(rl[3][it]+rl[3][it1]);
fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
fn[i0+6] = rl[2][it1]-rl[2][it];
fn[i0+7] = rl[3][it1]-rl[3][it];
}
fn[mh-4]=x1[0][mt-1];
fn[mh-3]=x1[1][mt-1]-ps0;
fn[mh-2]=x1[2][mt-1]-xi0;
fn[mh-1]=x1[3][mt-1]-et0;
////HENBIBUN////////////
//// I 0 //////
//// A B 0 ////
//// 0 A B 0 //
///////////////
////////////I 0
//I
fph[0][0]=one;
fph[1][1]=one;
fph[2][2]=one;
fph[3][3]=one;
//A
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*iu;
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=-one;
}
fph[i0+1][j0] = hdt;
fph[i0+2][j0+1] =-hdt*sps;
fph[i0+3][j0+1] = hdt*cps;
fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
fph[i0][j0+4] = hdt;
fph[i0+4][j0+5] =-hdt;
fph[i0+5][j0+6] = hdt*sps;
fph[i0+5][j0+7] =-hdt*cps;
}
//B
//j0=j0+mv;
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*(iu+1);
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=one;
}
fph[i0+1][j0]=hdt;
fph[i0+2][j0+1]=-hdt*sps;
fph[i0+3][j0+1]=hdt*cps;
fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
fph[i0][j0+4]=hdt;
fph[i0+4][j0+5]=-hdt;
fph[i0+5][j0+6]=hdt*sps;
fph[i0+5][j0+7]=-hdt*cps;
}
//I
fph[mh-4][mh-8]=one;
fph[mh-3][mh-7]=one;
fph[mh-2][mh-6]=one;
fph[mh-1][mh-5]=one;
//GYAKUGYOURETSU
for(int i=0;i<mh;i++){
for(int j=0;j<mh;j++){
fpi[i][j]=fph[i][j];
}
}
for(int j=0;j<mh;j++) {
for(int i=0;i<mh;i++) {
if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
}
}
for(int i=0;i<mh;i++) {
double q=wk[i];
// if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
wk[i]=1.0/q;
if(q > qmax){qmax=q;}
}
if(eps < 0){eps=qmax*mh*ueps;}
rdet =1.0;
idet =0.0;
for(int k=0;k<mh;k++) {
double amax = 0.0;
int kmax = k;
for(int i=k;i<mh;i++) {
if(fabs(fpi[i][k])*wk[i] <= amax){}
else{
amax=fabs(fpi[i][k])*wk[i];
kmax=i;
}
}
//if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
rdet=rdet*fpi[kmax][k];
if(k != kmax){rdet=-rdet;}
double adet =fabs(rdet);
while(adet > 1.0){
adet = adet*0.0625;
idet = idet + 4;
}
if(adet >= 0.0625){}
else{
adet=adet*16.0;
idet=idet-4;
}
if(rdet < 0.0){adet=-adet;}
rdet=adet;
iwk[k]=kmax;
amax=-1.0/fpi[kmax][k];
t=fpi[kmax][k];
fpi[kmax][k]=fpi[k][k];
fpi[k][k]=t;
for(int j=0;j<mh;j++) {
if(j == k){}
else{
t=fpi[kmax][j]*amax;
fpi[kmax][j]=fpi[k][j];
if(fabs(t) <= 0){}
else{
for(int i=0;i<mh;i++) {
fpi[i][j]=fpi[i][j]+t*fpi[i][k];
}
}
fpi[k][j]=-t;
}
}
for(int i=0;i<mh;i++) {
fpi[i][k]=fpi[i][k]*amax;
}
fpi[k][k]=-amax;
}
for(int l=0;l<mh;l++) {
int k=mh-l-1;
if(k == iwk[k]){}
else{
int kmax=iwk[k];
for(int i=0;i<mh;i++) {
t=fpi[i][k];
fpi[i][k]=fpi[i][kmax];
fpi[i][kmax]=t;
}
}
}
//////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
sum=sum-fpi[jh][ih]*fn[ih];
}
dph1[jh]=sum;
}
for(int it=0;it<mh;it++){
phi[it]= phf[it]+hal*dt1*dph1[it];
}
//k2
////SABUNSHIKI//////
for(int it=0;it<mt;it++){
int i0=mv*it;
x1[0][it]=phi[i0+0];
x1[1][it]=phi[i0+1];
x1[2][it]=phi[i0+2];
x1[3][it]=phi[i0+3];
rl[0][it]=phi[i0+4];
rl[1][it]=phi[i0+5];
rl[2][it]=phi[i0+6];
rl[3][it]=phi[i0+7];
}
fn[0]=x1[0][0];
fn[1]=x1[1][0];
fn[2]=x1[2][0];
fn[3]=x1[3][0];
for(int it=0;it<md;it++){
int i0=mx+mv*it;
int it1=it+1;
double bkp = hal*(x1[0][it]+x1[0][it1]);
double bps = hal*(x1[1][it]+x1[1][it1]);
double br1 = hal*(rl[0][it]+rl[0][it1]);
double br2 = hal*(rl[1][it]+rl[1][it1]);
double br3 = hal*(rl[2][it]+rl[2][it1]);
double br4 = hal*(rl[3][it]+rl[3][it1]);
fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
fn[i0+6] = rl[2][it1]-rl[2][it];
fn[i0+7] = rl[3][it1]-rl[3][it];
}
fn[mh-4]=x1[0][mt-1];
fn[mh-3]=x1[1][mt-1]-ps0;
fn[mh-2]=x1[2][mt-1]-xi0;
fn[mh-1]=x1[3][mt-1]-et0;
////HENBIBUN////////////
//// I 0 //////
//// A B 0 ////
//// 0 A B 0 //
///////////////
////////////I 0
//I
fph[0][0]=one;
fph[1][1]=one;
fph[2][2]=one;
fph[3][3]=one;
//A
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*iu;
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=-one;
}
fph[i0+1][j0] = hdt;
fph[i0+2][j0+1] =-hdt*sps;
fph[i0+3][j0+1] = hdt*cps;
fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
fph[i0][j0+4] = hdt;
fph[i0+4][j0+5] =-hdt;
fph[i0+5][j0+6] = hdt*sps;
fph[i0+5][j0+7] =-hdt*cps;
}
//B
//j0=j0+mv;
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*(iu+1);
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=one;
}
fph[i0+1][j0]=hdt;
fph[i0+2][j0+1]=-hdt*sps;
fph[i0+3][j0+1]=hdt*cps;
fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
fph[i0][j0+4]=hdt;
fph[i0+4][j0+5]=-hdt;
fph[i0+5][j0+6]=hdt*sps;
fph[i0+5][j0+7]=-hdt*cps;
}
//I
fph[mh-4][mh-8]=one;
fph[mh-3][mh-7]=one;
fph[mh-2][mh-6]=one;
fph[mh-1][mh-5]=one;
//GYAKUGYOURETSU
for(int i=0;i<mh;i++){
for(int j=0;j<mh;j++){
fpi[i][j]=fph[i][j];
}
}
for(int j=0;j<mh;j++) {
for(int i=0;i<mh;i++) {
if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
}
}
for(int i=0;i<mh;i++) {
double q=wk[i];
// if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
wk[i]=1.0/q;
if(q > qmax){qmax=q;}
}
if(eps < 0){eps=qmax*mh*ueps;}
rdet =1.0;
idet =0.0;
for(int k=0;k<mh;k++) {
double amax = 0.0;
int kmax = k;
for(int i=k;i<mh;i++) {
if(fabs(fpi[i][k])*wk[i] <= amax){}
else{
amax=fabs(fpi[i][k])*wk[i];
kmax=i;
}
}
// if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
rdet=rdet*fpi[kmax][k];
if(k != kmax){rdet=-rdet;}
double adet =fabs(rdet);
while(adet > 1.0){
adet = adet*0.0625;
idet = idet + 4;
}
if(adet >= 0.0625){}
else{
adet=adet*16.0;
idet=idet-4;
}
if(rdet < 0.0){adet=-adet;}
rdet=adet;
iwk[k]=kmax;
amax=-1.0/fpi[kmax][k];
t=fpi[kmax][k];
fpi[kmax][k]=fpi[k][k];
fpi[k][k]=t;
for(int j=0;j<mh;j++) {
if(j == k){}
else{
t=fpi[kmax][j]*amax;
fpi[kmax][j]=fpi[k][j];
if(fabs(t) <= 0){}
else{
for(int i=0;i<mh;i++) {
fpi[i][j]=fpi[i][j]+t*fpi[i][k];
}
}
fpi[k][j]=-t;
}
}
for(int i=0;i<mh;i++) {
fpi[i][k]=fpi[i][k]*amax;
}
fpi[k][k]=-amax;
}
for(int l=0;l<mh;l++) {
int k=mh-l-1;
if(k == iwk[k]){}
else{
int kmax=iwk[k];
for(int i=0;i<mh;i++) {
t=fpi[i][k];
fpi[i][k]=fpi[i][kmax];
fpi[i][kmax]=t;
}
}
}
//////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
sum=sum-fpi[jh][ih]*fn[ih];
}
dph2[jh]=sum;
}
//k3
for(int it=0;it<mh;it++){
phi[it]= phf[it]+hal*dt1*dph2[it];
}
////SABUNSHIKI//////
for(int it=0;it<mt;it++){
int i0=mv*it;
x1[0][it]=phi[i0+0];
x1[1][it]=phi[i0+1];
x1[2][it]=phi[i0+2];
x1[3][it]=phi[i0+3];
rl[0][it]=phi[i0+4];
rl[1][it]=phi[i0+5];
rl[2][it]=phi[i0+6];
rl[3][it]=phi[i0+7];
}
fn[0]=x1[0][0];
fn[1]=x1[1][0];
fn[2]=x1[2][0];
fn[3]=x1[3][0];
for(int it=0;it<md;it++){
int i0=mx+mv*it;
int it1=it+1;
double bkp = hal*(x1[0][it]+x1[0][it1]);
double bps = hal*(x1[1][it]+x1[1][it1]);
double br1 = hal*(rl[0][it]+rl[0][it1]);
double br2 = hal*(rl[1][it]+rl[1][it1]);
double br3 = hal*(rl[2][it]+rl[2][it1]);
double br4 = hal*(rl[3][it]+rl[3][it1]);
fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
fn[i0+6] = rl[2][it1]-rl[2][it];
fn[i0+7] = rl[3][it1]-rl[3][it];
}
fn[mh-4]=x1[0][mt-1];
fn[mh-3]=x1[1][mt-1]-ps0;
fn[mh-2]=x1[2][mt-1]-xi0;
fn[mh-1]=x1[3][mt-1]-et0;
////HENBIBUN////////////
//// I 0 //////
//// A B 0 ////
//// 0 A B 0 //
///////////////
////////////I 0
//I
fph[0][0]=one;
fph[1][1]=one;
fph[2][2]=one;
fph[3][3]=one;
//A
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*iu;
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=-one;
}
fph[i0+1][j0] = hdt;
fph[i0+2][j0+1] =-hdt*sps;
fph[i0+3][j0+1] = hdt*cps;
fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
fph[i0][j0+4] = hdt;
fph[i0+4][j0+5] =-hdt;
fph[i0+5][j0+6] = hdt*sps;
fph[i0+5][j0+7] =-hdt*cps;
}
//B
//j0=j0+mv;
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*(iu+1);
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=one;
}
fph[i0+1][j0]=hdt;
fph[i0+2][j0+1]=-hdt*sps;
fph[i0+3][j0+1]=hdt*cps;
fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
fph[i0][j0+4]=hdt;
fph[i0+4][j0+5]=-hdt;
fph[i0+5][j0+6]=hdt*sps;
fph[i0+5][j0+7]=-hdt*cps;
}
//I
fph[mh-4][mh-8]=one;
fph[mh-3][mh-7]=one;
fph[mh-2][mh-6]=one;
fph[mh-1][mh-5]=one;
//GYAKUGYOURETSU
for(int i=0;i<mh;i++){
for(int j=0;j<mh;j++){
fpi[i][j]=fph[i][j];
}
}
for(int j=0;j<mh;j++) {
for(int i=0;i<mh;i++) {
if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
}
}
for(int i=0;i<mh;i++) {
double q=wk[i];
// if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
wk[i]=1.0/q;
if(q > qmax){qmax=q;}
}
if(eps < 0){eps=qmax*mh*ueps;}
rdet =1.0;
idet =0.0;
for(int k=0;k<mh;k++) {
double amax = 0.0;
int kmax = k;
for(int i=k;i<mh;i++) {
if(fabs(fpi[i][k])*wk[i] <= amax){}
else{
amax=fabs(fpi[i][k])*wk[i];
kmax=i;
}
}
// if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
rdet=rdet*fpi[kmax][k];
if(k != kmax){rdet=-rdet;}
double adet =fabs(rdet);
while(adet > 1.0){
adet = adet*0.0625;
idet = idet + 4;
}
if(adet >= 0.0625){}
else{
adet=adet*16.0;
idet=idet-4;
}
if(rdet < 0.0){adet=-adet;}
rdet=adet;
iwk[k]=kmax;
amax=-1.0/fpi[kmax][k];
t=fpi[kmax][k];
fpi[kmax][k]=fpi[k][k];
fpi[k][k]=t;
for(int j=0;j<mh;j++) {
if(j == k){}
else{
t=fpi[kmax][j]*amax;
fpi[kmax][j]=fpi[k][j];
if(fabs(t) <= 0){}
else{
for(int i=0;i<mh;i++) {
fpi[i][j]=fpi[i][j]+t*fpi[i][k];
}
}
fpi[k][j]=-t;
}
}
for(int i=0;i<mh;i++) {
fpi[i][k]=fpi[i][k]*amax;
}
fpi[k][k]=-amax;
}
for(int l=0;l<mh;l++) {
int k=mh-l-1;
if(k == iwk[k]){}
else{
int kmax=iwk[k];
for(int i=0;i<mh;i++) {
t=fpi[i][k];
fpi[i][k]=fpi[i][kmax];
fpi[i][kmax]=t;
}
}
}
//////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
sum=sum-fpi[jh][ih]*fn[ih];
}
dph3[jh]=sum;
}
//k4
for(int it=0;it<mh;it++){
phi[it]= phf[it]+dt1*dph3[it];
}
////SABUNSHIKI//////
for(int it=0;it<mt;it++){
int i0=mv*it;
x1[0][it]=phi[i0+0];
x1[1][it]=phi[i0+1];
x1[2][it]=phi[i0+2];
x1[3][it]=phi[i0+3];
rl[0][it]=phi[i0+4];
rl[1][it]=phi[i0+5];
rl[2][it]=phi[i0+6];
rl[3][it]=phi[i0+7];
}
fn[0]=x1[0][0];
fn[1]=x1[1][0];
fn[2]=x1[2][0];
fn[3]=x1[3][0];
for(int it=0;it<md;it++){
int i0=mx+mv*it;
int it1=it+1;
double bkp = hal*(x1[0][it]+x1[0][it1]);
double bps = hal*(x1[1][it]+x1[1][it1]);
double br1 = hal*(rl[0][it]+rl[0][it1]);
double br2 = hal*(rl[1][it]+rl[1][it1]);
double br3 = hal*(rl[2][it]+rl[2][it1]);
double br4 = hal*(rl[3][it]+rl[3][it1]);
fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
fn[i0+6] = rl[2][it1]-rl[2][it];
fn[i0+7] = rl[3][it1]-rl[3][it];
}
fn[mh-4]=x1[0][mt-1];
fn[mh-3]=x1[1][mt-1]-ps0;
fn[mh-2]=x1[2][mt-1]-xi0;
fn[mh-1]=x1[3][mt-1]-et0;
////HENBIBUN////////////
//// I 0 //////
//// A B 0 ////
//// 0 A B 0 //
///////////////
////////////I 0
//I
fph[0][0]=one;
fph[1][1]=one;
fph[2][2]=one;
fph[3][3]=one;
//A
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*iu;
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=-one;
}
fph[i0+1][j0] = hdt;
fph[i0+2][j0+1] =-hdt*sps;
fph[i0+3][j0+1] = hdt*cps;
fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
fph[i0][j0+4] = hdt;
fph[i0+4][j0+5] =-hdt;
fph[i0+5][j0+6] = hdt*sps;
fph[i0+5][j0+7] =-hdt*cps;
}
//B
//j0=j0+mv;
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*(iu+1);
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=one;
}
fph[i0+1][j0]=hdt;
fph[i0+2][j0+1]=-hdt*sps;
fph[i0+3][j0+1]=hdt*cps;
fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
fph[i0][j0+4]=hdt;
fph[i0+4][j0+5]=-hdt;
fph[i0+5][j0+6]=hdt*sps;
fph[i0+5][j0+7]=-hdt*cps;
}
//I
fph[mh-4][mh-8]=one;
fph[mh-3][mh-7]=one;
fph[mh-2][mh-6]=one;
fph[mh-1][mh-5]=one;
//GYAKUGYOURETSU
for(int i=0;i<mh;i++){
for(int j=0;j<mh;j++){
fpi[i][j]=fph[i][j];
}
}
for(int j=0;j<mh;j++) {
for(int i=0;i<mh;i++) {
if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
}
}
for(int i=0;i<mh;i++) {
double q=wk[i];
// if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
wk[i]=1.0/q;
if(q > qmax){qmax=q;}
}
if(eps < 0){eps=qmax*mh*ueps;}
rdet =1.0;
idet =0.0;
for(int k=0;k<mh;k++) {
double amax = 0.0;
int kmax = k;
for(int i=k;i<mh;i++) {
if(fabs(fpi[i][k])*wk[i] <= amax){}
else{
amax=fabs(fpi[i][k])*wk[i];
kmax=i;
}
}
// if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
rdet=rdet*fpi[kmax][k];
if(k != kmax){rdet=-rdet;}
double adet =fabs(rdet);
while(adet > 1.0){
adet = adet*0.0625;
idet = idet + 4;
}
if(adet >= 0.0625){}
else{
adet=adet*16.0;
idet=idet-4;
}
if(rdet < 0.0){adet=-adet;}
rdet=adet;
iwk[k]=kmax;
amax=-1.0/fpi[kmax][k];
t=fpi[kmax][k];
fpi[kmax][k]=fpi[k][k];
fpi[k][k]=t;
for(int j=0;j<mh;j++) {
if(j == k){}
else{
t=fpi[kmax][j]*amax;
fpi[kmax][j]=fpi[k][j];
if(fabs(t) <= 0){}
else{
for(int i=0;i<mh;i++) {
fpi[i][j]=fpi[i][j]+t*fpi[i][k];
}
}
fpi[k][j]=-t;
}
}
for(int i=0;i<mh;i++) {
fpi[i][k]=fpi[i][k]*amax;
}
fpi[k][k]=-amax;
}
for(int l=0;l<mh;l++) {
int k=mh-l-1;
if(k == iwk[k]){}
else{
int kmax=iwk[k];
for(int i=0;i<mh;i++) {
t=fpi[i][k];
fpi[i][k]=fpi[i][kmax];
fpi[i][kmax]=t;
}
}
}
//////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
sum=sum-fpi[jh][ih]*fn[ih];
}
dph4[jh]=sum;
}
//saitekikai
for(int it=0;it<mh;it++){
phf[it]= phf[it]+dt1*(dph1[it]+2*dph2[it]+2*dph3[it]+dph4[it])/6.0;
}
}
for (int id=0; id< mt; ++id){
int ii = mt-id-1;
int i0 = mv*ii;
ap[id]=phi[i0+0];
ps[id]=phi[i0+1];
xi1[id]=phi[i0+2]*drf;
et1[id]=phi[i0+3]*drf;
xi[id]=xi1[id]-xi1[0];
et[id]=et1[id]-et1[0];
}
led1 = 1;
wait(1.0);
///////////////////
flag_lc = 1; // end of homotopy flag
// ******************** homotopy ********************
}
}
while(flag_cm<loop){
// ******************** sequence No.1 ********************
buf_char = mavc.getc();
buf_hex_rx[0] = (unsigned int)buf_char;
for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; }
if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x43 && buf_hex_rx[27]==0x4D && buf_hex_rx[26]==0x0D){
// ******************** sequence No.1 ********************
// ******************** sequence No.2 ********************
// send to mavc
mavc.putc(H);
for(int i=0;i<data;i++){ mavc.putc(D[i]); }
// ******************** sequence No.2 ********************
// ******************** sequence No.3 ********************
// buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 |
// |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY |
// | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] |
// transrating hex to dec
for(int i=0;i<10;i++){ buf_dec_rx[i] = buf_hex_rx[24-i*2]*256+buf_hex_rx[23-i*2]; }
for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec_rx[i]-32768)/65535*20*9.8; }
for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec_rx[i+3]-32768)/65535*600/180*pi; }
azi = (double)buf_dec_rx[6]/65535*2*pi;
alt = ((double)buf_dec_rx[7]-32768)*0.1;
for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec_rx[i+8]-32768)*0.1; }
// ******************** sequence No.3 ********************
// ******************** initialization *******************
for(int i=0;i<mt;i++){
dr[i]=i*drf/md;
}
if(flag_int == 0){
x0 = GPS[0];
y0 = GPS[1];
ps0 = azi;
ht0 = alt;
b0 = ht0;
b1 = tan(gm0);
b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf);
b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
flag_int=1;
}
xis = (GPS[0] - x0)*cos(pi/2-ps0)+(GPS[1] - y0)*sin(pi/2-ps0);
ets = -(GPS[0] - x0)*cos(pi/2-ps0)+(GPS[1] - y0)*sin(pi/2-ps0);
psr = rps0*dtr -(azi - ps0);
htr = alt;
avp = gyro[0];
avq = gyro[1];
avr = gyro[2];
// ******************** initialization *******************
// ******************** interpolation ********************
//xir ni hennkannseneba
//filter henko
if (xis != xis1 && fabs(xis-xis1)<50) {
s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1);
ua = 0.5*(ua+sqrt(s)/(time-time1));
drc = drc+ua*(time-time1);
drp= drc;
xir=xis + ua * dt * cos(psr);
etr=ets + ua * dt * sin(psr);
xis1=xis;
ets1=ets;
}
else{
drp = drp + ua * dt;
xir = xir + ua * dt * cos(psr);
etr = etr + ua * dt * sin(psr);
}
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
/*
b0 = ht0
b1 = (hf-ht0)/drf
*/
/*
htc = b0 + b1*drp + b2*drp*drp + b3*drp*drp*drp;
for(int i=0;i<mt;i++){
c = drp-dr[i];
c1= dr[i+1]-drp;
if(c >=0 && c1 >= 0){
c0=c/(dr[i+1]-dr[i]);
c1=c1/(dr[i+1]-dr[i]);
psc=c0*ps[i+1]+c1*ps[i];
xic=c0*xi[i+1]+c1*xi[i];
etc=c0*et[i+1]+c1*et[i];
break;
}
}
*/
htc = ht0;
xic = 0;
etc = 0;
// ******************** guidance law WO homo *************
// ******************** control law **********************
xie=xic-xir;
pse=(psc-psr)*dtr;
ete=etc-etr;
hte=htc-htr;
//horizontal
v2=vmr*vmr;
gkp=omg1*omg1/grv;
gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr;
phff=atan(apc*v2/grv);
phic=-gkp*ete-gkd*pse-phff;
avpc=(phic-phic00)/dt;
phic00=phic;
//vertical
a22=grv*sin(gmr*dtr)/vmr;
a23=0;
a32=vmr*cos(gmr*dtr);
b21=swg*rho*vmr*dclda*cos(phir*dtr)/(2*wgt);
aaa=a32*b21;
ck1=(a22+2*zet2*omg2)/aaa;
ck2=(a23*a32+omg2*omg2)/aaa;
dhdt=(hte-hte00)/dt;
// to translate deg/s to rad/s, add *pi/180. TORATANI
avqc=(ck1*dhdt+ck2*hte)/tt2;
hte00=hte;
com[0] = avpc;
com[1] = avqc;
com[2] = avrc;
if(fabs(hte) > 12 || fabs(xie) > 12 || fabs(ete) >12){
com[0] = 0;
com[1] = 0;
com[2] = 0;
}
// transrating dec to hex
for(int i=0;i<3;i++){
buf_dec_tx[i] = 19660.5/pi*com[i]+32768;
buf_hex_tx[i][0] = buf_dec_tx[i]/256;
buf_dec_tx[i] = buf_dec_tx[i]-buf_hex_tx[i][0]*256;
buf_hex_tx[i][1] = buf_dec_tx[i];
for(int j=0;j<2;j++){ D[j+2*i] = buf_hex_tx[i][j]; }
}
// ******************** control law **********************
// !!! HOSHINO !!! fprintf add guidance, velocity and control
fprintf(fp, "%6.1f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %8.5f %8.5f %8.1f %8.1f %8.1f %8.1f %8.1f %8.1f\r\n",time ,gyro[0],avpc, gyro[1],avqc, gyro[2],avrc, psr,psc, htr,htc, xir,xic,etr,etc);
time = time + 0.1;
flag_cm++;
}
}
fclose(fp);
led1 = 1; led2 = 1; led3 = 1; led4 = 1;
}