/****************************************************************************** // pitch and roll functions for Spray2008 SPI control // CS assignment = CS_PITCH, CS_ROLL is done in spi_cmd.h // created Oct2008 // updated:Feb2009 // //==== Future Expansion ======================================================= // last update: Feb11 // v0902.5 feb11 update roll heading control // // 1. // 2. debug menu & simplify the main menu // 3. verify that multiple calls won't conflict. // *****************************************************************************/ #include // Persistor BIOS and I/O Definitions #include // Persistor PicoDOS Definitions #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include // PicoDOS POSIX-like Directory Access Defines #include // PicoDOS DOS Drive and Directory Definitions #include // PicoDOS POSIX-like File Access Definitions #include // PicoDOS POSIX-like File Status Definitions #include // PicoDOS POSIX-like Terminal I/O Definitions #include // PicoDOS POSIX-like UNIX Function Definitions #include "cf2_qsm332.h" #include "spi_cmd.h" #include "gpio_mux.h" #include "tcm2.h" #include "dat_fnx.h" #include "pitch_roll.h" #include "setup.h" // globals extern char Verbose; // switch for displaying more info extern char IRQ_Flag; // interrupt flag: says what caused the interrupt extern short iparam [ MAX_PARAM ]; // global parameter settings extern struct all_param all; //--internal prototypes void pr_main_menu ( void ); void pr_dbg_menu ( void ); void pr_debug ( void ); //*** 24jun09, motor brake mode //***************************************************************************** short send_p_r_brake ( short cs, short on ) //****************************** { // send the command for braking the motor // cs = CS_PITCH or CS_ROLL // on = TRUE if want to turn on the brake, // on = FALSE to turn it OFF. // ON leaves the motor controller ON = higher current draw. // returns the final receive status: // SPI_ERR = error parsing the packet on the receive end // SPI_BUSY= peripheral is busy, try again later. // SPI_BAD = hardware error // SPI_OK = parsed OK // SPI_QUERY= parsed OK and request to send a data packet (from slave to master) short rstat; struct spi_param *ss; ss = all.sspi; //point to the spi struct setup_spi ( cs, ss ); // re-init the SPI info ss->mdat[0] = AUX_PWR; // control aux lines if (on) ss->mdat[1] = PR_ON_AD; //turn on A/D, which turns on 5V, 14V else ss->mdat[1] = -PR_ON_AD; // turns it off. ss->nbyte = 2; // #bytes to send rstat = spi_try_send( ss, 3 ); // try up to 3x to send return ( rstat ); } //*************************************************************************** //***************************************************************************** short send_p_r_run ( short cs, short rel, short cnts ) //******************* { // send the command for running the motor // cs = CS_PITCH or CS_ROLL // rel = relative(0) counts, absolute(1), or an index lookup( if cnts=0) // cnts = counts to move (see rel for interpretation) // returns the final receive status: // SPI_ERR = error parsing the packet on the receive end // SPI_BUSY= peripheral is busy, try again later. // SPI_BAD = hardware error // SPI_OK = parsed OK // SPI_QUERY= parsed OK and request to send a data packet (from slave to master) short rstat; short *w; struct spi_param *ss; ss = all.sspi; //point to the spi struct setup_spi ( cs, ss ); // re-init the SPI info // jul09 v3, added check to make sure it is off first. xfr_SPI_byte( cs, SPI_STOP ); // make sure it is presently stopped! ss->mdat[0] = MOTOR_RUN; // dat[0] = cmd byte ss->mdat[1] = rel; // interpretation of cnts w = (short*) &(ss->mdat[2]); // points to even addr to store short *w = cnts; // stores cnts at ss->mdat[2-3], big-endian format ss->nbyte = 4; // send the above rstat = spi_try_send( ss, 3 ); // try up to 3x to send return ( rstat ); } //*************************************************************************** short wait_p_r ( short cs, short tmax ) //************************************* { // wait up to tmax s for pitch/roll module to be complete // returns how long it actually waited short rstat, rwait; long tnow, t0, tstop; struct spi_param *ss; ss = all.sspi; setup_spi ( cs, ss ); // re-init the SPI info t0 = tnow = RTCGetTime ( 0, 0); tstop = t0 + tmax; do //--------------------------------------------------- { // keep trying to get an answer // or until timeout DelayMilliSecs(500); // wait rstat = spi_query( ss ); // get the reply rwait = ( (rstat == SPI_BUSY) || (rstat == SPI_OK) ); // SPI_OK can occur if the module JUST gets ready.. // ...catching it just waking up. // jul09 v3, catch rest of unexpected errors if (!rwait && rstat != SPI_CRC_OK ) { //,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, //unkown state, stop & get new reply xfr_SPI_byte( cs, SPI_STOP ); // STOP!! rstat = spi_query( ss ); // get the reply } //,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, if (Verbose) cprintf("\n wait_p_r rstat = %d\n", rstat); tnow = RTCGetTime ( 0, 0); } while ( rwait && tnowserr[3]); return ( nval ); } //*************************************************************************** //***************************************************************************** short last_ops ( short cs ) //************************************************* { // get the last motor operation statistics // returns TRUE if good, // returns 0 if failed (use to decide whether to dump to flash) short rstat, ch ; struct spi_param *ss; ss = all.sspi; setup_spi ( cs, ss ); // re-init the SPI info ss->mdat[0] = LAST_RQST; // dat[0] = cmd byte ss->nbyte = 1; // send the above byte rstat = spi_try_send ( ss, 2 ); // send the request DelayMilliSecs ( 20 ); rstat = spi_query( ss ); // get the reply ch = parse_query_ss( rstat, ss ); // ch is the command response if ( ch == MOTOR_RUN ) //======================================== { // then a succesful reply parse_last_ops ( ss ); //stick an a struct ch = ss->sdat[1] != MOTOR_CLEAR; // TRUE if a new op to report } else ch = 0; return ( ch ); } //*************************************************************************** short clear_ops ( short cs ) //************************************************ { // get the last motor operation statistics // returns MOTOR_RUN if good, // returns 0 if failed (use to decide whether to dump to flash) short rstat ; struct spi_param *ss; ss = all.sspi; setup_spi ( cs, ss ); // re-init the SPI info ss->mdat[0] = CLEAR_RQST; // dat[0] = cmd byte ss->nbyte = 1; // send the above byte rstat = spi_try_send ( ss, 3 ); // send the request return ( rstat ); } //*************************************************************************** void parse_last_ops ( struct spi_param *ss ) //******************************* { // parse the last operations request into the struct // it is then ready for display or writing to flash short *w; struct motor_stat *mot; mot = all.mot; // point to the motor stats mot->cs = ss->pcs; // either pitch or roll mot->result = ss->sdat[1]; // motor status w = (short*) &(ss->sdat[2]); // have w point to the next char value mot->pot_start = *w++; // start pot value mot->pot_stop = *w++; // stop pot value mot->amp = *w++; // avg amps mot->volt = *w++; // avg volts mot->ti = *w++; // time (0.1s incr) mot->limit = *w++; // final limit sw setting return; } //*************************************************************************** //***************************************************************************** short spi_motor ( short cs, short rel, short cnts ) //********************* { // send the command for running the motor // and wait for the result // cs = CS_PITCH or CS_ROLL // rel = relative(0) counts, absolute(1), or an index lookup( if cnts=0) // cnts = counts to move (see rel for interpretation) short rstat, rwait; short i=0; struct spi_param *ss; ss = all.sspi; //point to the spi struct rstat = send_p_r_run ( cs, rel, cnts ); // send the command if ( rstat != SPI_CRC_OK ) { // error sending the command cprintf("Err send_p_r_run = %d\n", rstat ); return ( rstat ); } // else OK, wait for the reply do //========================================================== { // check for a message every 0.5s DelayMilliSecs(500 ); // wait rstat = spi_query( ss ); // get the reply rwait = ( (rstat == SPI_BUSY) || (rstat == SPI_OK) ); // rwait=true if still need to wait display_pr_query (rstat, ss ); // display the results } while ( rwait && i++<20 ); //============================== if ( rstat != SPI_CRC_OK ) //---------------------------------- { // try one more time cprintf("bad rstat= %02x ", rstat); rstat = spi_query( ss ); // get the reply display_pr_query (rstat, ss ); // display the results } //----------------------------------------------------------- return ( rstat ); } //*************************************************************************** short exercise_motor( short cs ) //******************************************* { // bench-test exercise short i, n, k1, k2, n1, n2; struct spi_param *ss; ss = all.sspi; //point to the spi struct // re-init the SPI info ss->pcs = cs; // passed on to other functions init_qsm( cs ); init_spi(); //k1 = F_port + F_POT_SET; // limit 1 = this setting //k2 = F_stbd + F_POT_SET; // limit 2 = this setting // test limit switches k1 = F_MIN_POT; // limit 1 = this setting k2 = F_MAX_POT; // limit 2 = this setting n1 = 0; n2=0; // 0=absolute move k1=0; k2=0; n1=50; n2=-50; // k1=k2=0=relative, n1, n2=amount to move printf(" Give # of cycles to perform \n"); i = get_num( &n ); // get #cycles if (i<1) return 1; // failed ! printf("Resetting power accum.\n"); spi_pwr_rst( cs ); for (i=0;imotor_ph = 0; // start phase run_p_r_wait( cs, k1, n1 ); // send the command DelayMilliSecs(500 ); // wait printf("%3db ",i); ss->motor_ph = 1; // end phase run_p_r_wait( cs, k2, n2 ); // send the command DelayMilliSecs(500 ); // wait } //================================================================= printf("Done!, Final stats: \n"); spi_pwr_get( ss ); return 0; } //*************************************************************************** void display_pr_pwr ( struct spi_param *ss ) //****************************** { // display the power switch info short val, off, stat; val = ss->sdat[1]; // sensor channel switched off = ss->sdat[2]; // whether turned on or off stat= ss->sdat[3]; // whether it was successful printf("Motor Sensor ID %3d was turned ",val); if (off) cprintf("OFF"); else cprintf("ON"); if (stat>0) cprintf(" OK"); else cprintf(" FAILED"); printf("\n"); return ; } //*************************************************************************** void display_pr_ad12 ( struct spi_param *ss ) //****************************** { // display the a/d info short ch, val, *w; ch = ss->sdat[1]; // a/d chan w = ( short*) &(ss->sdat[2]); // point to even addr of msb val = *w; // read in the value // val = ss->sdat[2]*256 + ss->sdat[3]; // a/d value printf( "A/D " ); switch ( ch ) { // display channel string case AD_AMPS : cprintf("AMPS = "); break; case AD_VOLT : cprintf("VOLTS= "); break; case AD_POT : cprintf("POT = "); break; default : cprintf("UNKOWN[%d] = ",ch); } //======================================================= cprintf("%5d\n",val); return ; } //*************************************************************************** void display_motor ( struct spi_param *ss ) //******************************** { // display the motor result // uses what is in the global motor_stat function from parse_last_ops short merr, p1, p2, jt, amp, volt, limit, k1, k2, cs; short ph; struct motor_stat *mot; mot = all.mot; // point to the motor stats cs = mot->cs; // either pitch or roll merr = mot->result; // motor status p1 = mot->pot_start; // start pot value p2 = mot->pot_stop; // stop pot value amp = mot->amp; // avg amps volt = mot->volt; // avg volts jt = mot->ti; // time (0.1s incr) limit = mot->limit; // final limit sw setting ph = ss->motor_ph; // phase of motor exercise, used for display purposes k1 =limit&0x01; // set TRUE if first bit is set k2 =limit&0x02; // = 2 if 2nd bit is set k2 = k2>>1; // and shift over, = 0 or 1 if ( cs == CS_PITCH ) cprintf("Pitch "); if ( cs == CS_ROLL ) cprintf("Roll "); cprintf( "Motor " ); switch ( merr ) { // display motor status case MOTOR_OK : cprintf("Ran OK, "); break; case MOTOR_DT : cprintf("Ran on time-only "); break; case MOTOR_BAD_POT: cprintf("BAD POT; ti-only "); break; case MOTOR_NO_MOVE: cprintf("BAD POT; NO OP "); break; case MOTOR_NONE : cprintf("NO RUN, <3 cnts "); break; case MOTOR_SLOW : cprintf("Too-slow over 2s "); break; case MOTOR_MAX : cprintf("Above Max cnts "); break; case MOTOR_MIN : cprintf("Below Min cmts "); break; case MOTOR_TMO : cprintf("timed-out (slow) "); break; case MOTOR_AMPS : cprintf("current>max "); break; case MOTOR_LIMIT : cprintf("At limit switch "); break; case MOTOR_STOP : cprintf("External Stop "); break; case MOTOR_CLEAR : cprintf("Last OP cleared "); break; default : cprintf("UNKOWN[%d] = ",merr); } //======================================================= if (ph==0) { // display start pot first printf(" start pot=%5d, end pot=%5d amp=%5d v=%5d time= %4.1f s lim=%d%d \n", p1, p2, amp, volt, jt*0.1, k1,k2 ); } else { // display end pot first printf(" end pot=%5d, start pot=%5d amp=%5d v=%5d time= %4.1f s lim=%d%d\n", p2, p1, amp, volt, jt*0.1, k1,k2 ); } return ; } //*************************************************************************** void display_pr_query (short rstat, struct spi_param *ss ) //******************** { // display the query results short spi_cmd; spi_cmd = parse_query_ss(rstat, ss); switch ( spi_cmd ) //================================================ { // display results accordingly case 0 : break; // not valid! case SPI_VER : display_ver ( ); break; case AUX_PWR : display_pr_pwr ( ss ); break; case SPI_MEASURE : display_pr_ad12 ( ss ); break; case PWR_RQST : display_pwr ( ss ); break; case MOTOR_RUN : parse_last_ops ( ss ); display_motor ( ss ); break; case FL_BLOCK_RD : display_dump ( ss ); break; default : display_dump ( ss ); break; } //================================================================= return; } //*************************************************************************** void hpr_pwr_get ( short cs ) //********************************************* { // get the cs module power accumulator values and store short err; short i, *w; short tia, tavg, watts; struct spi_param *ss; ss = all.sspi; setup_spi( cs, ss ); // initializes the SPI info err = spi_pwr_get ( ss ); if (err == SPI_CRC_OK ) //===================================== { // then a successful return, parse data w = ( short*) &(ss->sdat[0]); // point to even addr of msb i=1; // index of the first value tia = w[i++]; // accumulated time [s] tavg = w[i++]; // accumulated time [s] that power was estimated watts= w[i++]; // average 0.01 watts //store to the structure if ( cs == CS_HYD ) { all.hyd->vac = vacuum(); // store the vacuum all.hyd->total = tia; // gets stored in hyd struct as well all.hyd->watt = watts; i=0; } else if ( cs == CS_PITCH) { i = 1; } else { i = 2; } // default is roll all.pwr->watt[i] = watts; all.pwr->ti[i] = tia; } //=========================================================== // also store the two battery voltages // this is mainly so V7 gets sent back via satellite, // but also sends back v14 as measured under no pump load i = r_get_a2d (CS_GPIO, AD_AUX_V_07 ); all.pwr->v7 = c2eng(i, W_07V); i = r_get_a2d (CS_GPIO, AD_AUX_V_14 ); all.pwr->v14 = c2eng(i, W_14V); return; } //*************************************************************************** void motor_menu ( short cs ) //******************************************** { // display options for running/testing the motor // runs the module selected by cs short nval, ncnts, n1, n2, wait; struct spi_param *ss; ss = all.sspi; cprintf(" 0 : Move DC counts from present position \n" ); cprintf(" 1 : Move to ABSOLUTE user-specified counts \n"); cprintf(" OR specify a pre-set value: \n"); /* cprintf(" %d : ZERO setting (zero roll or level pitch) \n", F_zero ); cprintf(" %d : PORT setting (port wing up or XMIT pitch) \n", F_port ); cprintf(" %d : STBD setting (stbd wing up) \n\n", F_stbd ); /**/ cprintf(" 10 : MIN setting \n"); cprintf(" 12 : MAX setting \n"); cprintf(" Give index (0=relative move, 1=abs) \n"); n1 = get_num(&nval); if (n1==1 && nval <2 ) { // then relative move, get counts cprintf(" give counts \n"); n2 = get_num(&ncnts); } // else index of value to goto else { ncnts = 0; n2=1; } cprintf("Wait for result (Y/N)? \n"); wait = YesNo(); if (n1==1 && n2==1 ) //then a legal read of value { // send command //====================================== switch ( nval ) { // make sure it is legal /* case F_zero : case F_port : case F_stbd : // these are relative to an address nval += F_POT_SET; // correct address break; /**/ case 10 : // goto the min setting nval = F_MIN_POT; break; case 12 : // goto max setting nval = F_MAX_POT; break; case 0 : // relative move, should be set break; case 1 : break; // OK, 1=absolute, ncnts=abs. value default : printf("ILLEGAL entry \n"); return; // exit early } // end switch ss->motor_ph = 0; // display as if start of cycle if (wait) run_p_r_wait( cs, nval,ncnts ); // send the command else send_p_r_run( cs, nval,ncnts ); // send the command } //====================================================== else { cprintf(" BAD input value \n"); } return ; }//**************************************************************************** void pr_flash_menu ( struct spi_param *ss ) //******************************** { // display options for running/testing the motor short nval, ncnts, n1, n2; printf(" Give flash-eeprom addr to update \n" ); printf(" %2d : MSP430 Board Serial Number \n", F_SER_NO ); printf(" %2d : #Motor counts change per second \n", F_DCDT ); printf(" %2d : Min potentiometer counts \n", F_MIN_POT ); printf(" %2d : Max potentiometer counts \n", F_MAX_POT ); printf(" %2d : milli-Amps = (F_C2U_I * counts/1000)\n", F_C2U_I ); printf(" %2d : Volts*100 = (F_C2U_V * counts/100)\n", F_C2U_V ); printf(" %2d : Max current (mA) before shutting off motor\n", F_MAX_I ); /* printf(" %2d : ZERO setting (zero roll or level pitch) \n", F_zero+F_POT_SET ); printf(" %2d : PORT setting (port wing up or XMIT pitch) \n", F_port+F_POT_SET ); printf(" %2d : STBD setting (stbd wing up) \n\n", F_stbd+F_POT_SET ); */ printf(" Give address \n"); n1 = get_num(&nval); if (n1 ==1 ) { printf(" Give the new value to store \n"); n2 = get_num(&ncnts); } if (n1 ==0 || n2 ==0 ) { printf(" ILLEGAL VALUE \n"); return; } // else proceed switch ( nval ) { // make sure it is legal case F_SER_NO : case F_DCDT : case F_MIN_POT : case F_MAX_POT : case F_C2U_I : case F_C2U_V : case F_MAX_I : /* case (F_zero +F_POT_SET ) : case (F_port +F_POT_SET ) : case (F_stbd +F_POT_SET ) : /**/ // these are all legal values spi_flash_write( nval, ncnts, ss ); break; default : printf("ILLEGAL address \n"); } // end switch return ; }//**************************************************************************** void pr_set_default ( struct spi_param *ss ) //******************************* { // apr11: set pitch/roll flash values to default // this is used for a brand-new board short nval, n1; cprintf(" Give New MSP430 serial # \n"); n1 = get_num(&nval); if (n1 !=1 || nval<1 || nval>32767 ) { cprintf(" ILLEGAL s/n\n"); return; } // else proceed spi_flash_write( F_SER_NO, nval, ss ); DelayMilliSecs(500); spi_flash_write( F_DCDT , 17, ss ); // 17 counts per sec DelayMilliSecs(500); spi_flash_write( F_MIN_POT, 1800, ss ); // Min potentiometer counts DelayMilliSecs(500); spi_flash_write( F_MAX_POT, 2200, ss ); // Max potentiometer counts DelayMilliSecs(500); spi_flash_write( F_C2U_I , 16, ss ); // milli-Amps = (F_C2U_I * counts/1000) DelayMilliSecs(500); spi_flash_write( F_C2U_V , 68, ss ); // Volts*100 = (F_C2U_V * counts/100) DelayMilliSecs(500); spi_flash_write( F_MAX_I , 200, ss ); // Max current (mA) before shutting off motor DelayMilliSecs(500); // display contents init_pr_iparam( ss->pcs ); return ; }//**************************************************************************** short init_pr_iparam ( short cs ) //****************************************** { // read in the pitch or roll module flash data block, // and assign the iparam[] arrays accordingly short rstat; // rstat = rqst_flash_0 ( cs ); // outdated, ea module now returns all info in ver rstat = spi_cmd_ver ( cs ); if (rstat==0) display_prflash (); else cprintf("%d Error getting MSP430 flash info from cs=%d\n", rstat, cs); return ( rstat ); } //*************************************************************************** void display_prflash ( void ) //********************************************** { // display flash eeprom values // first flash data (addr=0 offset in MSP430) is at sdat[2] // msp430 is lsb, msb convention short sn, ver, dcdt, imin, imax, c2i, c2v, amp; short *w, val=0, i=0; struct spi_param *ss; ss = all.sspi; ver = ss->sdat[1]; // code-compilation version w = ( short * ) &( ss->sdat[2] ); // flash data starts at sdat[2] i=0; sn = w[i++]; // serial # dcdt= w[i++]; // counts change per sec imin= w[i++]; // min pot counts imax= w[i++]; // max pot counts c2i = w[i++]; // cnts 2 amps conversion factor c2v = w[i++]; // cnts 2 volts conversion factor amp = w[i++]; // max mA motor current allowed // store to the appropriate iparam settings cprintf("MSP430 EEPROM settings for the "); if ( ss->pcs == CS_PITCH ) //============================================= { // store to the pitch params iparam [ MA_pi_sn ] = sn; iparam [ MA_pi_ver ] = ver; iparam [ MA_pitch_dcdt ] = dcdt; iparam [ MA_pi_max_amps] = amp; iparam [ MA_pitch_min ] = imin; iparam [ MA_pitch_max ] = imax; iparam [ MA_pi_c2i ] = c2i; iparam [ MA_pi_c2v ] = c2v; cprintf( "PITCH "); } else if ( ss->pcs == CS_ROLL ) //========================================= { // store to the roll params iparam [ MA_ro_sn ] = sn; iparam [ MA_ro_ver ] = ver; iparam [ MA_roll_dcdt ] = dcdt; iparam [ MA_ro_max_amps] = amp; iparam [ MA_roll_min ] = imin; iparam [ MA_roll_max ] = imax; iparam [ MA_ro_c2i ] = c2i; iparam [ MA_ro_c2v ] = c2v; cprintf( "ROLL "); } //===================================================================== cprintf("module\n"); cprintf(" %4d : MSP430 Board Serial Number \n", sn ); cprintf(" %4d : MSP430 Firmware Version # \n" , ver ); cprintf(" %4d : #Motor counts change per second \n", dcdt ); cprintf(" %4d : Min potentiometer counts \n", imin ); cprintf(" %4d : Max potentiometer counts \n", imax ); cprintf(" %4d : Amps = cnts * F_C2U_I / 1000 \n", c2i ); cprintf(" %4d : Volts = cnts * F_C2U_V / 1000 \n", c2v ); cprintf(" %4d : Max motor current [mA] \n", amp ); return ; }//**************************************************************************** short get_p_r_pot ( short cs ) //********************************************** { // send commands to pitch or roll module to sample the pitch pot // returns the value // returns -1 if no response // this is now a call to the generic a2d function short cnt; power_on_off( cs, PR_ON_AD, 1 ); // turn on the a/d power DelayMilliSecs( 20 ); cnt = r_get_a2d ( cs, AD_POT ); return ( cnt ); } //*************************************************************************** //flight control functions **************************************************** //***************************************************************************** void control_pitch (float now, short desired, float g ) //****************** { // change pitch from now to desired, in degrees // g = fractional change to gain,used to underestimate (bias low) // this SENDS the command, but does NOT wait for it to complete // other commands to the pitch module while this one is running will // be ignored. float err, dc, gain; short ierr, idc; gain = iparam [ IA_pitch_10d ]; // #counts per 10 degrees gain = gain*0.1 * g; // biased counts per degree //TCM2 returns a positive angle for the nose up err = desired - now; // error in pitch : >0 if want more nose up // to NOSE UP requires moving pack AFT, requires idc<0 dc = -err*gain; //=guessed # counts to move idc = 0.8*dc; // underestimate & recast as an integer if ( idc> 200) idc= 200; // limit max change if ( idc<-200) idc=-200; printf("control_pitch now=%4.1f want=%3d, dc=%d\n",now,desired, idc); if (Verbose) printf("Pitch angle err=%4.1f, gain=%4.1f, dc=%5d\n",err,gain,idc); if (abs(idc)>3) // big enough to move ------------------------ { ierr = send_p_r_run (CS_PITCH, 0, idc ); if (ierr != SPI_CRC_OK) cprintf(" PITCH problem in control_pitch %d\n", ierr); } //---------------------------------------------------------- return; } //*************************************************************************** void control_roll ( float now, short desired ) //**************************** { // change roll from now to desired, in degrees roll, from compass // gain = #counts to change roll by 1 deg., gain = cnts/degree // this WAITS for the roll to be completed, // so there is no synch problems float err, dc, g; short ti, idc; g = (float) iparam [ IA_roll_10d ]; g = g*0.1; // roll gain = #counts to change roll by 1 degree // TCM2 has negative angle with port wing down err = desired - now; // error in roll : <0 if want more port wing down dc = -err*g ; //=guessed # counts to move idc = 0.8*dc; // underestimate & recast as an integer printf("control_roll now=%4.1f want=%3d, dc=%d\n",now,desired, idc); if (Verbose) printf("Roll angle err=%4.1f, gain=%4.1f, dc=%5d\n",err,g,idc); if (abs(idc)>3) // big enough to move ------------------------- { ti = run_p_r_wait (CS_ROLL, 0, idc ); if (Verbose) cprintf(" time to roll to %d degrees = %d s\n", desired, ti); } //------------------------------------------------------------ return; } //*************************************************************************** //***************************************************************************** void control_head ( short down, float dh, float ravg ) //******************** { // change roll to go towards desired heading : // down = true if descending, false if ascending // dh = present heading error = ACTUAL - DESIRED // ravg = average roll over the last period // esum is the integrated heading error // other parameters // IA_ro_dn = roll angle to go straight on descent // IA_ro_up = roll angle to go straight on ascent // IA_ro_use = use roll angle on the feedback // IA_ro_rtau = roll divider for weighting the heading error // IA_ro_etau = roll divider for weighting the integrated error // rg = roll gain = #counts change per degree roll float dr, r, rg, rtau, etau, r0d, r0u, em, rm ; short ierr,idc, cflag, esum, ro_use; struct flight_param *fp; fp = all.flt; esum = fp->esum; // =integrated heading error rg = (float) (iparam [ IA_roll_10d ] )*0.1; // #cnts to change roll by 1 deg rtau = (float) (iparam [ IA_ro_rtau ] )*0.1; // scale factor for head error // feb11, convert proportional scale factor from denominator to numerator rm = 0; // initialize if ( rtau != 0) rm = 1.0/rtau; //now = multiplier: jan16, allow negative //-- repeat for I time constant --------- etau = (float) (iparam [ IA_ro_etau ] )*0.1; // scale factor for int. head error em = 0; // 1/etau scale factor: default to DO NOT USE //-- feb11: if etau >=100, keep em=0 = do NOT use integrated value if ( etau<100 && etau !=0 ) em = 1.0 /etau; // jan16, allow negative values r0d = (float) (iparam [ IA_ro_dn ] ); // roll angle to go straight, descent r0u = (float) (iparam [ IA_ro_up ] ); // roll angle to go straight, ascent // r = R_0 - dh/rtau - esum/etau; // r0d, r0u = guessed roll req'd to go straight on descent/ascent // fp->tau = time constant on how quickly to correct dh // fp->etau = time constant on how quickly to correct the integrated error rm = rm/fp->tau; // feb11 new weight, corrected for the scale factor. em = em/fp->etau; // feb11, divide em by fp->etau fp->em = em; //feb11, save a copy for calculating new roll corrections fp->rm = rm; //feb11, save a copy // if dh>0 or esum>0, we need to turn ccw to correct // for going down, this means port wing down : r<0 if (down) { r = r0d - dh*rm - esum*em; } else // we're going up, r>0 causes ccw turn { r = r0u + dh*rm + esum*em; } // now make sure we don't roll too far if (r> 30) r= 30; //don't go past +30 deg. if (r<-30) r=-30; //don't go past -30 deg. // up to this point, we've guessed the amount of roll given PI error // now make the correction, based on using actual roll angle, or not. ro_use = iparam [ IA_ro_use ] & RO_USE; // feb11, >0 if want to use roll if ( ro_use ) //+++++++++++++++++++++++++++++++++++++++++++++++++ { // then we believe the roll angle sensor dr = r - ravg; //= amount of roll change desired idc = (short) (-dr*rg); //= amount of roll counts change desired cflag = 0; // counts = relative change } else //+++++++++++++++++++++++++++++++++++++++++++++++++++ { // we don't believe the angle // r>0 = port-wing up, = counts decreasing from roll_0 // so if r>0, want to decrease counts idc = (short) (-r*rg) + iparam [ IA_roll_0 ]; // est. absolute counts cflag = 1; // counts is absolute value } //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // now roll to this value printf("dh=%6.1f, rnow=%6.1f, rnew=%6.1f idc=%d\n",dh, ravg, r, idc ); if ( abs(idc)>3 ) // it is big enough to correct { //ierr = send_p_r_run ( CS_ROLL, cflag, idc ); // jul09 v3, run the roll & wait for completion // this should make sure there is no a/d sampling problems ierr = run_p_r_wait ( CS_ROLL, cflag, idc ); //idc>0 means roll the port wing down if (ierr != SPI_CRC_OK ) cprintf(" ROLL problem in control head %d\n", ierr ); } //**end if big enough to move return; } //*************************************************************************** short roll_wing ( short wing_now, short seek ) //***************************** { // roll desired wing up : allow multiple tries // seek = TRUE if want to seek zero roll if pot is not good // set to FALSE if don't want to seek. // returns 1 if pot is good, else 0 //24jun09 added brake_on if port or stbd wing up short pot_want, pot_zero, pot, ti_ran, j; short dc, dc0, adc0, cmin, cmax, pot_good, i, err; short big_dc=0, dcdt, ro_use; short brake_on =0; ro_use = iparam [ IA_ro_use ] & RO_USE; // feb11, >0 if want to use roll pot_zero = iparam [ IA_roll_0 ]; // roll value for zero roll pot = get_p_r_pot ( CS_ROLL ); // get present pot value cmin = iparam [ MA_roll_min ]; // read in min val cmax = iparam [ MA_roll_max ]; // read in max val dcdt = iparam [ MA_roll_dcdt ]; if ( dcdt(cmin-200)) && (pot<(cmax+200) ) ); // we believe the pot switch (wing_now) //----------------------------------------- { // get the potentiometer counts for the desired wing position case PORT_WING : // port wing up pot_want = iparam [ IA_roll_port ]; brake_on = 1; // yes, turn it on break; case STBD_WING : // stbd wing up pot_want = iparam [ IA_roll_stbd ]; brake_on = 1; // yes, turn on the brake break; case ZERO_WING : // zero roll: wings flat pot_want = pot_zero; brake_on = 0; // NO, don't need the brake break; default : cprintf(" input unknown \n"); pot_want = pot_zero; } // end switch -------------------------------------------- if (pot_good) //+++++++++++++++++++++++++++++++++++++++++++++++++++++ { // try a few times to get the right wing up dc0 = dc = pot_want - pot; // amount to change the pot adc0 = abs(dc0); //absolute value i=0; do //------------------------------------------------------ { // attempt a roll : multiple times if walking wounded ti_ran = run_p_r_wait ( CS_ROLL, 0, dc ); //0=relative j=0; do { // jul09 v3 try up to 4x to get a pot reading // this is important, as it decides the next step pot = get_p_r_pot ( CS_ROLL ); } while (pot<0 && j++<4 ); pot_good = ( (pot>(cmin-200)) && (pot<(cmax+200) ) ); if (pot_good) //ppppppppppppppppppppppppppppppppppppp { // we believe the pot, compute how well we did dc = pot_want - pot; // amount still to go adc0 = abs(dc); // absolute value big_dc = adc0 > 20; // amount still to go. } else //pppppppppppppppppppppppppppppppppppppppppppppp { // bad pot half-way through, just guess a time // ti_ran = #s motor ran before the bad pot val. adc0 = adc0 - ti_ran*dcdt; // remaining amount if (dc0<0) adc0 = -adc0; // give the correct sign cprintf("Bad Pot, guessing to move %d counts\n",adc0); err = send_p_r_run(CS_ROLL, 0, adc0 ); // and do the roll all.roll_stat |= 0x80; // set MSbit: intermittent pot } // else one-time try at fixing bad pot pppppppppppp } while ( big_dc && i++<4 && pot_good ); //---------------- } // end if good pot ++++++++++++++++++++++++++++++++++++++++++++++++ else if ( seek && ro_use )//feb11 +++++++++++++++++++++++++++++++++++ { // pot is not good, just use a time estimate if wanted zero_roll_angle(); // this is our best attempt to roll flat dc = pot_want - pot_zero; // estimate of how much to roll run_p_r_wait ( CS_ROLL, 0, dc ); // the roll module uses time-only when the roll pot is bad } // end if..else +++++++++++++++++++++++++++++++++++++++++++++++++++ if (brake_on==1) cprintf("setting roll brake\n"); send_p_r_brake ( CS_ROLL, brake_on ); // the brake will remain on until the next wing roll command if (big_dc) { // then we did not make it to the desired value all.roll_stat |= ( 0x10 << wing_now ); // left-shift the 3rd bit // set bit[4] if wing_now=0 = STBD_WING // bit[5] if wing_now=1 = PORT_WING // bit[6] if wing_now=2 = ZERO_WING } // end if err return(pot_good); } //*************************************************************************** void zero_roll_angle(void) //************************************************** { // Use compass to get to zero roll angle // Used if the roll potentiometer is broken. short i=0; float aphi, phi; // roll angle do //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ { // multiple tries to get to zero based on roll angle phi = avg_tcm2(3, ANG_ROLL); // = average roll angle, 3 samples aphi = flt_abs(phi); control_roll ( phi, 0 ); // at angle phi, try to get to 0 } while ( i++<6 && aphi>3 ); //++++++++++++++++++++++++++++++++++++++++ printf("zero-roll phi=%6.1f ntries=%d\n\n",phi,i); return; } //*************************************************************************** void zero_pitch_angle(void) //************************************************* { // Use compass to get to servo to a zero pitch angle // Used if the pitch potentiometer is broken. short i=0; float aphi, phi; if ( iparam [ IA_pi_stat ] <2 ) //------------------------------- { // then potentiometer is healthy, Pitch_Xmit; // go to pre-set value for zero pitch return; } //-------------------------------------------------------------- do //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ { // multiple tries to get to zero based on roll angle phi = avg_tcm2 ( 3, ANG_PITCH ); // = average PITCH angle, 3 samples aphi = flt_abs(phi); if (aphi<48) control_pitch(phi, 0, 0.8 ); // try to move to zero wait_p_r ( CS_PITCH, 30 ); // wait for pitch to be done } while ( i++<6 && aphi>3 ); //++++++++++++++++++++++++++++++++++++++++ printf("zero-pitch phi=%6.1f ntries=%d\n\n",phi,i); return; } //*************************************************************************** short RF_Wing_Setup( short unit ) // ***************************************** { // given the unit type (GPS or SBD), configure wing + antenna switch // returns 1=pot is good, else 0 (0711) short w_now=0, w_start=0, pot_good; if (unit== GPS_UNIT) //---------------------------------------------- w_start = iparam [ IA_wing0_GPS ]; // For GPS, use this wing else // must be SBD_UNIT -------------------------------------------- { unit = SBD_UNIT; // if it's not GPS, make sure it's an SBD w_start = iparam [ IA_wing0_SBD ]; // For SBD, use this wing } //----------------------------------------------------------------- // override these settings if we're in the abort sequence if (all.exc->abort) // then we're in the abort code, w_start = ALT_WING; // force toggling between wings w_now = all.wing_now[unit]; // = the present setting if (w_now !=PORT_WING) w_now = STBD_WING; // make a legal value switch(w_start) //--------------------------------------------------- { // choose the correct wing, dependent upon desired eeprom value case PORT_WING : w_now = PORT_WING; break; case STBD_WING : w_now = STBD_WING; break; case ALT_WING : w_now = !w_now; break; // alternate default : w_now = STBD_WING; break; // default } // end select ----------------------------------------------------- if (w_now != PORT_WING) w_now = STBD_WING; // make sure legal all.wing_now[unit] = w_now; // save the setting pot_good = roll_wing( w_now, 1 ); // roll the right wing up, 1=seek=TRUE ant_switch(unit,w_now); // config ant switch for right unit, wing DelayMilliSecs(200); // wait for the switch to settle return( pot_good ); } //*************************************************************************** void test_pitch_roll ( void ) //********************************************* { // test either the pitch or roll module char ch; //input char short done, cs, val, tmo; struct spi_param *ss; ss = all.sspi; cs = CS_PITCH; // allows cs to be toggled between pitch/roll printf(" Test of Pitch and Roll : %s \n", __DATE__); pr_main_menu(); done = false; ch =-1; //illegal input char tmo = 0; while (!done) //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ { //cycle until we get a quit command // re-init the SPI info if ( cs == CS_PITCH ) { if ( ch>0) cprintf("Pitch Module is selected....\n"); } else if ( cs == CS_ROLL ) { if ( ch>0) cprintf("Roll Module is selected....\n"); } else { cs = CS_PITCH; cprintf("Illegal CS, default to Pitch module\n"); } if (ch !=' ' && ch>0 ) cprintf("M_1>\n"); // display directory prompt setup_spi( cs, ss ); EIAEnableRx(1); while ( SCIRxQueuedCount() ) SCIRxFlush(); // delete any old stuff ch = -1; ch = SCIRxGetCharWithTimeout(1000 ); // get a new character tmo = check_tmo ( &ch, tmo ); // increments the tmo counter if ch<0, and // goes to sleep mode after max_tmo. Returns *ch as upper-case //================================================================ switch(ch) //==================================================== { // and depending upon the ch, do the appropriate action ======= case 'Q' : done=1; break; //quit function case 'U' : pr_main_menu(); break; case '?' : break; // causes directory prompt to be displayed case ' ' : break; // wait for another character case -1 : break; // wait for another character case '+' : toggle_verbose(); // toggle verbose mode break; case 'A' : // a/d sample val = r_get_a2d ( cs, AD_POT ); // get the pot cnts cprintf("Pot counts = %6d\n", val); break; case 'B' : // brake control cprintf("Turning brake on.\n"); send_p_r_brake( cs, 1 ); break; case 'D' : pr_debug(); break; case 'L' : last_ops ( cs ); display_motor ( ss ); break; case 'M' : // run the motor motor_menu ( cs ); break; case 'P' : cs = CS_PITCH ; break; case 'R' : cs = CS_ROLL ; break; case 'S' : spi_send_stop ( cs ); break; case 'V' : val = spi_cmd_ver ( cs ); if (val==0) display_ver (); break; case '0' : Pitch_Xmit; break; case '1' : Pitch_Level; break; case '2' : Roll_Zero; break; case '3' : Roll_Port_W_Up; break; case '4' : Roll_Stbd_W_Up; break; default : printf(" input unknown \n"); } //============================================================== } // end while !done //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ send_p_r_brake( CS_ROLL , 0 ); send_p_r_brake( CS_PITCH, 0 ); cprintf("Made sure that the brake-mode is OFF\n"); return; } //*************************************************************************** void pr_main_menu ( void ) { //print out menu selections ************************************************* printf(" Pitch and Roll Menu\n" " Q.uit menu : U.pdate menu \n" " + Toggle Verbose: V.ersion of SPI : S.top Motor!! \n" " P.itch Select : R.oll Select : B.rake Control \n" " A.pot reading : M.otor Control : L.ast reading \n\n" " 0=Pitch_Xmit, 1=Pitch_Level, 2=Roll_Zero, 3=Port_W_Up, 4=Stbd_W_Up\n"); return; } //*************************************************************************** //***debug functions ********************************************************** void pr_debug ( void ) //*************************************************** { // test either the pitch or roll module char ch; //input char short done, cs, val, tmo; struct spi_param *ss; ss = all.sspi; cs = CS_PITCH; // allows cs to be toggled between pitch/roll printf(" Debug Pitch and Roll : %s \n", __DATE__); pr_dbg_menu(); done = false; ch =-1; //illegal input char tmo = 0; while (!done) //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ { //cycle until we get a quit command // re-init the SPI info if ( cs == CS_PITCH ) { if ( ch>0) cprintf("Pitch Module is selected....\n"); } else if ( cs == CS_ROLL ) { if ( ch>0) cprintf("Roll Module is selected....\n"); } else { cs = CS_PITCH; cprintf("Illegal CS, default to Pitch module\n"); } if (ch !=' ' && ch>0 ) cprintf("M_2>\n"); // display directory prompt setup_spi( cs, ss ); EIAEnableRx(1); while ( SCIRxQueuedCount() ) SCIRxFlush(); // delete any old stuff ch = -1; ch = SCIRxGetCharWithTimeout(1000 ); // get a new character tmo = check_tmo ( &ch, tmo ); // increments the tmo counter if ch<0, and // goes to sleep mode after max_tmo. Returns *ch as upper-case //================================================================ switch(ch) //==================================================== { // and depending upon the ch, do the appropriate action ======= case 'Q' : done=1; break; //quit function case 'U' : pr_dbg_menu(); break; case '?' : break; // causes directory prompt to be displayed case ' ' : break; // wait for another character case -1 : break; // wait for another character case '+' : toggle_verbose(); // toggle verbose mode break; case 'A' : // a/d sample val = r_get_a2d ( cs, AD_POT ); // get the pot cnts cprintf("Pot counts = %6d\n", val); break; case 'B' : // brake control cprintf("Turning brake on.\n"); send_p_r_brake( cs, 1 ); break; case 'C' : clear_ops( cs ); break; case 'D' : // 64-byte information dump & save in iparam val = init_pr_iparam ( cs ); break; case 'F' : // write one word to flash pr_flash_menu( ss ); break; case 'G' : val = spi_query ( ss ); display_pr_query (val, ss ); // display the results break; case 'J' : hpr_pwr_get ( cs ); break; case 'L' : last_ops ( cs ); display_motor ( ss ); break; case 'M' : // run the motor motor_menu ( cs ); break; case 'P' : cs = CS_PITCH ; break; case 'R' : cs = CS_ROLL ; break; case 'S' : spi_send_stop ( cs ); break; case 'V' : val = spi_cmd_ver ( cs ); if (val==0) display_ver (); break; case 'X' : val = exercise_motor ( cs ); break; case 'Y' : val = last_a2d ( cs, AD_POT ); cprintf("last a/d val for chan %d is %d\n", AD_POT, val); break; case 'Z' : spi_pwr_rst ( cs ); cprintf("Pwr accumulators now reset\n"); break; case '0' : Pitch_Xmit; break; case '1' : Pitch_Level; break; case '2' : Roll_Zero; break; case '3' : Roll_Port_W_Up; break; case '4' : Roll_Stbd_W_Up; break; case '8' : val = xfr_SPI_byte(cs, SPI_RESET); cprintf("soft reset return = %c",val); if (val==' ') cprintf("OK"); cprintf("\n"); break; case '!' : // set flash msp430 eeprom to default values cprintf("Do you really want to reset flash to default (y/n)? >\n"); if (YesNo() ) { pr_set_default ( ss ); } break; default : printf(" input unknown \n"); } //============================================================== } // end while !done //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ send_p_r_brake( CS_ROLL , 0 ); send_p_r_brake( CS_PITCH, 0 ); cprintf("Made sure that the brake-mode is OFF\n"); return; } //*************************************************************************** void pr_dbg_menu ( void ) { //print out menu selections ************************************************* printf(" Debug Pitch and Roll Menu\n" " Q.uit menu : U.pdate menu \n" " A./D pot : D.ump Flash : F.lash write word \n" " M.otor Run : V.ersion of SPI : L.ast operation stats \n" " G.et queued info: + Toggle Verbose: X.ercise motor\n" " P.itch Select : R.oll Select : C.lear last op buffer\n" " J.oules Avg Get : Z.ero Avg Accum : S.top Motor!!!\n\n" " Y.last a/d : B.rake Control\n" " 8=Soft Reset : !.SET FLASH TO DEFAULT\n"); return; } //*************************************************************************** //*** END *********************************************************************