//Main Function int main(void) { double error_p = 0.0; double error_i = 0.0; double error_d = 0.0; double angle; kalman_state k = kalman_init(1, 16, 1, 137); init_devices(); lcd_set_4bit(); lcd_init(); double preverror = 0.0; while(1) { sensor2 = ADC_Conversion(2); kalman_update(&k,(double)sensor2); angle = k.x; lcd_print(1,5,(int)angle,3); error_p = (angle - 137); error_i += error_p*dt; error_d = (error_p - preverror)/dt; lcd_print(1,1,abs((int)error_p),3); Disturbance = ((error_p*kp) + (error_i*ki) + (error_d*kd)); if(angle > 137) back(); else { forward();} if (Disturbance > MAX) Disturbance = MAX; if (Disturbance < MIN) Disturbance = MIN; lcd_print(2,6,abs((int)Disturbance),5); velocity(35+abs((int)Disturbance),35+abs((int)Disturbance)); preverror = error_p; } }
int main (void){ float acc_x, acc_z, gyro_x; float acc_angle,kal_angle; /* Init Systick to 1ms */ SysTick_Config( SystemCoreClock / 1000); /* Initialize GPIO (sets up clock) */ LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6); SERVO_init(); if(I2CInit(I2CMASTER) == FALSE){ while(1); /* fatal error */ } if(MPU6050_init()){ return 0; } MPU6050_setZero(); kalman_init(); for(;;){ /* 100Hz loop */ if(gSysTick_10 >= 9){ gSysTick_10 = 0; /* get sensor values */ gyro_x = MPU6050_getGyroRoll_degree(); acc_x = MPU6050_getAccel_x(); acc_z = -MPU6050_getAccel_z(); /* acc angle */ acc_angle = atan2(acc_x , -acc_z) * 180/3.14159 ; // calculate accel angle kal_angle = kalman_update(acc_angle,gyro_x, 0.01); SERVO_set_slew((-kal_angle) - MECH_OFFSET); } } }
int main(int argc, char **argv) { int i; float gyro_input; float accel_input; float kalman_output; for (i = 0; i < SAMPLE_COUNT; ++i) { // Get the gyro and accelerometer input. gyro_input = sample_data[i][0]; accel_input = sample_data[i][1]; // Update the Kalman filter and get the output. kalman_output = kalman_update(gyro_input, accel_input); // Print out input data and the kalman output. printf("%d gyro=%7.3f deg/sec accel=%7.3f deg kalman_output=%5.1f deg\n", i, gyro_input, accel_input, kalman_output); } return 0; }
/***************************************************************************** * Balance - *****************************************************************************/ void balance(void) { unsigned long TimerMsWork; long int g_bias = 0; double x_offset = 532; //offset value 2.56V * 1024 / 4.93V = 4254 double q_m = 0.0; double int_angle = 0.0; double x = 0.0; double tilt = 0.0; int pwm; InitADC(); init_pwm(); // initialize the UART (serial port) uartInit(); // set the baud rate of the UART for our debug/reporting output uartSetBaudRate(115200); // initialize rprintf system rprintfInit(uartSendByte); // initialize vt100 library vt100Init(); // clear the terminal screen vt100ClearScreen(); TimerMsWork = TimerMsCur(); DDRB |= (1 << PB0); // Make B0 an output for LED /* as a 1st step, a reference measurement of the angular rate sensor is * done. This value is used as offset compensation */ for (int i=1 ; i<=200; i++) // determine initial value for bias of gyro { g_bias = g_bias + GetADC(gyro_sensor); } g_bias = g_bias / 200; while (!(getkey() == 1)) { /* insure loop runs at specified Hz */ while (!TimerCheck(TimerMsWork, (dt_PARAM * 1000) -1)) ; TimerMsWork = TimerMsCur(); // toggle pin B0 for oscilloscope timings. PORTB = PINB ^ (1 << PB0); // get rate gyro reading and convert to deg/sec // q_m = (GetADC(gyro_sensor) - g_bias) / -3.072; // -3.07bits/deg/sec (neg. because forward is CCW) q_m = (GetADC(gyro_sensor) - g_bias) * -0.3255; // each bit = 0.3255 /deg/sec (neg. because forward is CCW) state_update(q_m); // get Accelerometer reading and convert to units of gravity. // x = (GetADC(accel_sensor) - x_offset) / 204.9; // (205 bits/G) x = (GetADC(accel_sensor) - x_offset) * 0.00488; // each bit = 0.00488/G // x is measured in multiples of earth gravitation g // therefore x = sin (tilt) or tilt = arcsin(x) // for small angles in rad (not deg): arcsin(x)=x // Calculation of deg from rad: 1 deg = 180/pi = 57.29577951 tilt = 57.29577951 * (x); kalman_update(tilt); int_angle += angle * dt_PARAM; rprintf(" x:"); rprintfFloat(8, x); rprintf(" angle:"); rprintfFloat(8, angle); rprintf(" rate:"); rprintfFloat(8, rate); // Balance. The most important line in the entire program. // balance_torque = Kp * (current_angle - neutral) + Kd * current_rate; // rprintf("bal_torq: "); // rprintfFloat(8, balance_torque); // rprintfCRLF(); //steer_knob = 0; // change from current angle to something proportional to speed // should this be the abs val of the cur speed or just curr speed? //double steer_cmd = (1.0 / (1.0 + Ksteer2 * fabs(current_angle))) * (Ksteer * steer_knob); //double steer_cmd = 0.0; // Get current rate of turn //double current_turn = left_speed - right_speed; //<-- is this correct //double turn_accel = current_turn - prev_turn; //prev_turn = current_turn; // Closed-loop turn rate PID //double steer_cmd = KpTurn * (current_turn - steer_desired) // + KdTurn * turn_accel; // //+ KiTurn * turn_integrated; // Possibly optional //turn_integrated += current_turn - steer_cmd; // Differential steering //left_motor_torque = balance_torque + steer_cmd; //+ cur_speed + steer_cmd; //right_motor_torque = balance_torque - steer_cmd; //+ cur_speed - steer_cmd; // Limit extents of torque demand //left_motor_torque = flim(left_motor_torque, -MAX_TORQUE, MAX_TORQUE); // if (left_motor_torque < -MAX_TORQUE) left_motor_torque = -MAX_TORQUE; // if (left_motor_torque > MAX_TORQUE) left_motor_torque = MAX_TORQUE; //right_motor_torque = flim(right_motor_torque, -MAX_TORQUE, MAX_TORQUE); // if (right_motor_torque < -MAX_TORQUE) right_motor_torque = -MAX_TORQUE; // if (right_motor_torque > MAX_TORQUE) right_motor_torque = MAX_TORQUE; pwm = (int) ((angle -3.5) * Kp) + (rate * Kd); // + (int_angle * Ki); rprintf(" pwm:%d\r\n", pwm); // Set PWM values for both motors SetLeftMotorPWM(pwm); SetRightMotorPWM(pwm); } SetLeftMotorPWM(0); SetRightMotorPWM(0); }
/** Time domain physical simulation. noisy: - 0: no noise at all; - 1: poisson and read out noise. - 2: only poisson noise. */ dmat *skysim_sim(dmat **mresout, const dmat *mideal, const dmat *mideal_oa, double ngsol, ASTER_S *aster, const POWFS_S *powfs, const PARMS_S *parms, int idtratc, int noisy, int phystart){ int dtratc=0; if(!parms->skyc.multirate){ dtratc=parms->skyc.dtrats->p[idtratc]; } int hasphy; if(phystart>-1 && phystart<aster->nstep){ hasphy=1; }else{ hasphy=0; } const int nmod=mideal->nx; dmat *res=dnew(6,1);/*Results. 1-2: NGS and TT modes., 3-4:On axis NGS and TT modes, 4-6: On axis NGS and TT wihtout considering un-orthogonality.*/ dmat *mreal=NULL;/*modal correction at this step. */ dmat *merr=dnew(nmod,1);/*modal error */ dcell *merrm=dcellnew(1,1);dcell *pmerrm=NULL; const int nstep=aster->nstep?aster->nstep:parms->maos.nstep; dmat *mres=dnew(nmod,nstep); dmat* rnefs=parms->skyc.rnefs; dcell *zgradc=dcellnew3(aster->nwfs, 1, aster->ngs, 0); dcell *gradout=dcellnew3(aster->nwfs, 1, aster->ngs, 0); dmat *gradsave=0; if(parms->skyc.dbg){ gradsave=dnew(aster->tsa*2,nstep); } SERVO_T *st2t=0; kalman_t *kalman=0; dcell *mpsol=0; dmat *pgm=0; dmat *dtrats=0; int multirate=parms->skyc.multirate; if(multirate){ kalman=aster->kalman[0]; dtrats=aster->dtrats; }else{ if(parms->skyc.servo>0){ const double dtngs=parms->maos.dt*dtratc; st2t=servo_new(merrm, NULL, 0, dtngs, aster->gain->p[idtratc]); pgm=aster->pgm->p[idtratc]; }else{ kalman=aster->kalman[idtratc]; } } if(kalman){ kalman_init(kalman); mpsol=dcellnew(aster->nwfs, 1); //for psol grad. } const long nwvl=parms->maos.nwvl; dcell **psf=0, **mtche=0, **ints=0; ccell *wvf=0,*wvfc=0, *otf=0; if(hasphy){ psf=mycalloc(aster->nwfs,dcell*); wvf=ccellnew(aster->nwfs,1); wvfc=ccellnew(aster->nwfs,1); mtche=mycalloc(aster->nwfs,dcell*); ints=mycalloc(aster->nwfs,dcell*); otf=ccellnew(aster->nwfs,1); for(long iwfs=0; iwfs<aster->nwfs; iwfs++){ const int ipowfs=aster->wfs[iwfs].ipowfs; const long ncomp=parms->maos.ncomp[ipowfs]; const long nsa=parms->maos.nsa[ipowfs]; wvf->p[iwfs]=cnew(ncomp,ncomp); wvfc->p[iwfs]=NULL; psf[iwfs]=dcellnew(nsa,nwvl); //cfft2plan(wvf->p[iwfs], -1); if(parms->skyc.multirate){ mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[(int)aster->idtrats->p[iwfs]]; }else{ mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[idtratc]; } otf->p[iwfs]=cnew(ncomp,ncomp); //cfft2plan(otf->p[iwfs],-1); //cfft2plan(otf->p[iwfs],1); ints[iwfs]=dcellnew(nsa,1); int pixpsa=parms->skyc.pixpsa[ipowfs]; for(long isa=0; isa<nsa; isa++){ ints[iwfs]->p[isa]=dnew(pixpsa,pixpsa); } } } for(int irep=0; irep<parms->skyc.navg; irep++){ if(kalman){ kalman_init(kalman); }else{ servo_reset(st2t); } dcellzero(zgradc); dcellzero(gradout); if(ints){ for(int iwfs=0; iwfs<aster->nwfs; iwfs++){ dcellzero(ints[iwfs]); } } for(int istep=0; istep<nstep; istep++){ memcpy(merr->p, PCOL(mideal,istep), nmod*sizeof(double)); dadd(&merr, 1, mreal, -1);/*form NGS mode error; */ memcpy(PCOL(mres,istep),merr->p,sizeof(double)*nmod); if(mpsol){//collect averaged modes for PSOL. for(long iwfs=0; iwfs<aster->nwfs; iwfs++){ dadd(&mpsol->p[iwfs], 1, mreal, 1); } } pmerrm=0; if(istep>=parms->skyc.evlstart){/*performance evaluation*/ double res_ngs=dwdot(merr->p,parms->maos.mcc,merr->p); if(res_ngs>ngsol*100){ dfree(res); res=NULL; break; } { res->p[0]+=res_ngs; res->p[1]+=dwdot2(merr->p,parms->maos.mcc_tt,merr->p); double dot_oa=dwdot(merr->p, parms->maos.mcc_oa, merr->p); double dot_res_ideal=dwdot(merr->p, parms->maos.mcc_oa, PCOL(mideal,istep)); double dot_res_oa=0; for(int imod=0; imod<nmod; imod++){ dot_res_oa+=merr->p[imod]*IND(mideal_oa,imod,istep); } res->p[2]+=dot_oa-2*dot_res_ideal+2*dot_res_oa; res->p[4]+=dot_oa; } { double dot_oa_tt=dwdot2(merr->p, parms->maos.mcc_oa_tt, merr->p); /*Notice that mcc_oa_tt2 is 2x5 marix. */ double dot_res_ideal_tt=dwdot(merr->p, parms->maos.mcc_oa_tt2, PCOL(mideal,istep)); double dot_res_oa_tt=0; for(int imod=0; imod<2; imod++){ dot_res_oa_tt+=merr->p[imod]*IND(mideal_oa,imod,istep); } res->p[3]+=dot_oa_tt-2*dot_res_ideal_tt+2*dot_res_oa_tt; res->p[5]+=dot_oa_tt; } }//if evl if(istep<phystart || phystart<0){ /*Ztilt, noise free simulation for acquisition. */ dmm(&zgradc->m, 1, aster->gm, merr, "nn", 1);/*grad due to residual NGS mode. */ for(int iwfs=0; iwfs<aster->nwfs; iwfs++){ const int ipowfs=aster->wfs[iwfs].ipowfs; const long ng=parms->maos.nsa[ipowfs]*2; for(long ig=0; ig<ng; ig++){ zgradc->p[iwfs]->p[ig]+=aster->wfs[iwfs].ztiltout->p[istep*ng+ig]; } } for(int iwfs=0; iwfs<aster->nwfs; iwfs++){ int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc); if((istep+1) % dtrati==0){ dadd(&gradout->p[iwfs], 0, zgradc->p[iwfs], 1./dtrati); dzero(zgradc->p[iwfs]); if(noisy){ int idtrati=(multirate?(int)aster->idtrats->p[iwfs]:idtratc); dmat *nea=aster->wfs[iwfs].pistat->sanea->p[idtrati]; for(int i=0; i<nea->nx; i++){ gradout->p[iwfs]->p[i]+=nea->p[i]*randn(&aster->rand); } } pmerrm=merrm;//record output. } } }else{ /*Accumulate PSF intensities*/ for(long iwfs=0; iwfs<aster->nwfs; iwfs++){ const double thetax=aster->wfs[iwfs].thetax; const double thetay=aster->wfs[iwfs].thetay; const int ipowfs=aster->wfs[iwfs].ipowfs; const long nsa=parms->maos.nsa[ipowfs]; ccell* wvfout=aster->wfs[iwfs].wvfout[istep]; for(long iwvl=0; iwvl<nwvl; iwvl++){ double wvl=parms->maos.wvl[iwvl]; for(long isa=0; isa<nsa; isa++){ ccp(&wvfc->p[iwfs], IND(wvfout,isa,iwvl)); /*Apply NGS mode error to PSF. */ ngsmod2wvf(wvfc->p[iwfs], wvl, merr, powfs+ipowfs, isa, thetax, thetay, parms); cembedc(wvf->p[iwfs],wvfc->p[iwfs],0,C_FULL); cfft2(wvf->p[iwfs],-1); /*peak in corner. */ cabs22d(&psf[iwfs]->p[isa+nsa*iwvl], 1., wvf->p[iwfs], 1.); }/*isa */ }/*iwvl */ }/*iwfs */ /*Form detector image from accumulated PSFs*/ double igrad[2]; for(long iwfs=0; iwfs<aster->nwfs; iwfs++){ int dtrati=dtratc, idtrat=idtratc; if(multirate){//multirate idtrat=aster->idtrats->p[iwfs]; dtrati=dtrats->p[iwfs]; } if((istep+1) % dtrati == 0){/*has output */ dcellzero(ints[iwfs]); const int ipowfs=aster->wfs[iwfs].ipowfs; const long nsa=parms->maos.nsa[ipowfs]; for(long isa=0; isa<nsa; isa++){ for(long iwvl=0; iwvl<nwvl; iwvl++){ double siglev=aster->wfs[iwfs].siglev->p[iwvl]; ccpd(&otf->p[iwfs],psf[iwfs]->p[isa+nsa*iwvl]); cfft2i(otf->p[iwfs], 1); /*turn to OTF, peak in corner */ ccwm(otf->p[iwfs], powfs[ipowfs].dtf[iwvl].nominal); cfft2(otf->p[iwfs], -1); dspmulcreal(ints[iwfs]->p[isa]->p, powfs[ipowfs].dtf[iwvl].si, otf->p[iwfs]->p, siglev); } /*Add noise and apply matched filter. */ #if _OPENMP >= 200805 #pragma omp critical #endif switch(noisy){ case 0:/*no noise at all. */ break; case 1:/*both poisson and read out noise. */ { double bkgrnd=aster->wfs[iwfs].bkgrnd*dtrati; addnoise(ints[iwfs]->p[isa], &aster->rand, bkgrnd, bkgrnd, 0,0,IND(rnefs,idtrat,ipowfs)); } break; case 2:/*there is still poisson noise. */ addnoise(ints[iwfs]->p[isa], &aster->rand, 0, 0, 0,0,0); break; default: error("Invalid noisy\n"); } igrad[0]=0; igrad[1]=0; double pixtheta=parms->skyc.pixtheta[ipowfs]; if(parms->skyc.mtch){ dmulvec(igrad, mtche[iwfs]->p[isa], ints[iwfs]->p[isa]->p, 1); } if(!parms->skyc.mtch || fabs(igrad[0])>pixtheta || fabs(igrad[1])>pixtheta){ if(!parms->skyc.mtch){ warning2("fall back to cog\n"); }else{ warning_once("mtch is out of range\n"); } dcog(igrad, ints[iwfs]->p[isa], 0, 0, 0, 3*IND(rnefs,idtrat,ipowfs), 0); igrad[0]*=pixtheta; igrad[1]*=pixtheta; } gradout->p[iwfs]->p[isa]=igrad[0]; gradout->p[iwfs]->p[isa+nsa]=igrad[1]; }/*isa */ pmerrm=merrm; dcellzero(psf[iwfs]);/*reset accumulation.*/ }/*if iwfs has output*/ }/*for wfs*/ }/*if phystart */ //output to mreal after using it to ensure two cycle delay. if(st2t){//Type I or II control. if(st2t->mint->p[0]){//has output. dcp(&mreal, st2t->mint->p[0]->p[0]); } }else{//LQG control kalman_output(kalman, &mreal, 0, 1); } if(kalman){//LQG control int indk=0; //Form PSOL grads and obtain index to LQG M for(int iwfs=0; iwfs<aster->nwfs; iwfs++){ int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc); if((istep+1) % dtrati==0){ indk|=1<<iwfs; dmm(&gradout->p[iwfs], 1, aster->g->p[iwfs], mpsol->p[iwfs], "nn", 1./dtrati); dzero(mpsol->p[iwfs]); } } if(indk){ kalman_update(kalman, gradout->m, indk-1); } }else if(st2t){ if(pmerrm){ dmm(&merrm->p[0], 0, pgm, gradout->m, "nn", 1); } servo_filter(st2t, pmerrm);//do even if merrm is zero. to simulate additional latency } if(parms->skyc.dbg){ memcpy(PCOL(gradsave, istep), gradout->m->p, sizeof(double)*gradsave->nx); } }/*istep; */ } if(parms->skyc.dbg){ int dtrati=(multirate?(int)dtrats->p[0]:dtratc); writebin(gradsave,"%s/skysim_grads_aster%d_dtrat%d",dirsetup, aster->iaster,dtrati); writebin(mres,"%s/skysim_sim_mres_aster%d_dtrat%d",dirsetup,aster->iaster,dtrati); } dfree(mreal); dcellfree(mpsol); dfree(merr); dcellfree(merrm); dcellfree(zgradc); dcellfree(gradout); dfree(gradsave); if(hasphy){ dcellfreearr(psf, aster->nwfs); dcellfreearr(ints, aster->nwfs); ccellfree(wvf); ccellfree(wvfc); ccellfree(otf); free(mtche); } servo_free(st2t); /*dfree(mres); */ if(mresout) { *mresout=mres; }else{ dfree(mres); } dscale(res, 1./((nstep-parms->skyc.evlstart)*parms->skyc.navg)); return res; }
/** * ahrs_step() takes the values from the IMU and produces the * new estimated attitude. */ void ahrs_step( v_t angles_out, f_t dt, f_t p, f_t q, f_t r, f_t ax, f_t ay, f_t az, f_t heading ) { m_t C; m_t A; m_t AT; v_t X_dot; m_t temp; v_t Xm; v_t Xe; /* Construct the quaternion omega matrix */ rotate2omega( A, p, q, r ); /* X_dot = AX */ mv_mult( X_dot, A, X, 4, 4 ); /* X += X_dot dt */ v_scale( X_dot, X_dot, dt, 4 ); v_add( X, X_dot, X, 4 ); v_normq( X, X ); /* printf( "X =" ); Vprint( X, 4 ); printf( "\n" ); */ /* AT = transpose(A) */ m_transpose( AT, A, 4, 4 ); /* P = A * P * AT + Q */ m_mult( temp, A, P, 4, 4, 4 ); m_mult( P, temp, AT, 4, 4, 4 ); m_add( P, Q, P, 4, 4 ); compute_c( C, X ); /* Compute the euler angles of the measured attitude */ accel2euler( Xm, ax, ay, az, heading ); /* * Compute the euler angles of the estimated attitude * Xe = quat2euler( X ) */ quat2euler( Xe, X ); /* Kalman filter update */ kalman_update( P, R, C, X, Xe, Xm ); /* Estimated attitude extraction */ quat2euler( angles_out, X ); }