yal kaiyo
still homotopy @FB
Fork of
130111_YNU_MPU_4
by 
yal kaiyo
main.cpp
- Committer:
- yal_kaiyo
- Date:
- 2013-01-10
- Revision:
- 10:fe98c33623b6
- Parent:
- 9:a56c340f47c9
- Child:
- 11:5a744949ba5a
File content as of revision 10:fe98c33623b6:
#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 = 1200; // 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 xis = 0;
float ets = 0;
float psis = 0;
float x0 = 0;
float y0 = 0;
float drf = 100; //syuutandaunrenji
float ht0 = 50;
float htf = 5;
float gm0 = 20*pi/180;
float gmf = 5*pi/180;
float b0 = ht0;
float b1 = tan(gm0);
float b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf);
float b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
float htc = 0;
float x = 0; //down range
float ua = 10; //initial horizontal velosity
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;
//homotopy
float rps0 = 90.0;
float rxi0 = -600.0; //terminal xi
float ret0 = -200.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.0;
float rdet =1.0;
float idet =0.0;
float t = 0.0;
float phf[mh]={0};
float ap[mt]={0.0};
float ps[mt]={0.0};
float xi[mt]={0.0};
float et[mt]={0.0};
float xi1[mt]={0.0};
float et1[mt]={0.0};
float psc;
float etc;
float xic;
//********************* 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 ete = 0;
float hte = 0;
float hte00 = 0;
float dhdt = 0;
float v2 = 0;
float gkp = 0;
float gkd = 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(t == 0){
x0 = GPS[0];
y0 = GPS[1];
ps0 = azi;
ht0 = alt;
}
ets = (GPS[0] - x0)*cos(ps0);
xis = (GPS[1] - y0)*sin(ps0);
psr = pi/2 -(azi - ps0);
htr = 50+(alt-ht0);
avp = gyro[0];
avq = gyro[1];
avr = gyro[2];
// ******************** initialization *******************
// ******************** interpolation ********************
drp = drp + ua * dt;
xir = xir + ua * dt * cos(psr);
etr = etr + ua * dt * sin(psr);
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;
etr=ets;
xis1=xis;
ets1=ets;
}
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
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;
}
}
// ******************** guidance law WO homo *************
// ******************** control law **********************
pse=(psc-psr)*dtr;
ete=etc-etr;
hte=htc-htr;
//horizontal
/*
gkp=omg1*omg1/grv;
gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr;
avpc=1/tt1*(gkd*pse+gkp*ete);//
*/
//avpc=kp*hte;
//vertical
/*
v2=vmr*vmr;
dhdt=(hte-hte00)/dt
*/
// to translate deg/s to rad/s, add *pi/180. TORATANI
//avqc=(-1/tt2*(2/(0.084*rho*v2*swg))*(wgt*grv*cos(gmr*dtr)-wgt*(2*zet2*omg2*dhdt+omg2*omg2*hte)))*pi/180;
//hte00=hte;
//avqc=kq*(pse+ete);
// dummy output
avpc = avpc+0.001*pi;
avqc = 0;
avrc = avrc-0.001*pi;
com[0] = avpc;
com[1] = avqc;
com[2] = avrc;
// 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 %8.5f %8.1f %8.1f %8.1f\r\n",time, gyro[0], gyro[1], gyro[2], azi, alt, GPS[0], GPS[1]);
time = time + 0.1;
flag_cm++;
}
}
fclose(fp);
led1 = 1; led2 = 1; led3 = 1; led4 = 1;
}