/******************************************************************************
// 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	<cfxbios.h>		// Persistor BIOS and I/O Definitions
#include	<cfxpico.h>		// Persistor PicoDOS Definitions

#include	<assert.h>
#include	<ctype.h>
#include	<errno.h>
#include	<float.h>
#include	<limits.h>
#include	<locale.h>
#include	<math.h>
#include	<setjmp.h>
#include	<signal.h>
#include	<stdarg.h>
#include	<stddef.h>
#include	<stdio.h>
#include	<stdlib.h>
#include	<string.h>
#include	<time.h>

#include	<dirent.h>		// PicoDOS POSIX-like Directory Access Defines
#include	<dosdrive.h>	// PicoDOS DOS Drive and Directory Definitions
#include	<fcntl.h>		// PicoDOS POSIX-like File Access Definitions
#include	<stat.h>		// PicoDOS POSIX-like File Status Definitions
#include	<termios.h>		// PicoDOS POSIX-like Terminal I/O Definitions
#include	<unistd.h>		// 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 && tnow<tstop ); //--------------------
     
     display_pr_query (rstat, ss ); // display the results

     tnow = tnow - t0; // change in time
     rstat = (short) tnow; // re-cast as short
  
  return ( rstat );
} //***************************************************************************
  
short run_p_r_wait ( short cs, short rel, short cnts ) //**********************
{ // run the pitch/roll motor and wait for completion
  // returns the time (s) to run
  // returns -2 = not valid response to the command
  
  short rstat, nval, tmax, dcdt;
  
  
  // get the appropriate motor speed rate dc/dt
  if ( cs == CS_PITCH )
      nval = MA_pitch_dcdt;
  else 
      nval = MA_roll_dcdt;
      
  dcdt = iparam [ nval ]; 
  if ( dcdt<DCDT_R ) dcdt = DCDT_R; // keep above a minumum value!!!
  if ( rel==0 ) // then a relative move
     tmax = abs(cnts)/dcdt + 4; // max #seconds to wait  
  else // an absolute move
     tmax = 60; // else wait no more than 60 s
     
  if (Verbose)
     cprintf("Will run the motor and monitor for %d s\n", tmax);
  
  rstat = send_p_r_run ( cs, rel, cnts );
  if ( rstat == SPI_CRC_OK ) //========================================
  { // then command was sent successfully
  
     nval = wait_p_r ( cs, tmax ); // wait for completion
     
  } //=================================================================
  else nval=-2; // set that it failed
  
  // if (Verbose)
     cprintf("Final motor run-time = %6d, err= %3d\n", nval, all.exc->serr[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;i<n;i++) //=================================================
  { // perform n cycles
     printf("%3da  ",i);
     ss->motor_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<DCDT_R ) dcdt = DCDT_R; // keep above a minumum value!!!
  pot_good = (  (pot>(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 *********************************************************************
