/************************************************************************ ** Message decode example. To be Compiled for the PC Navigation computer. ** Decode of messages received from the Inertial Solutions (IS) control computer. ** ** RFL stands for Radio Frequency Link. The RFL is a 9600 bps serial port ** on the IS computer that communicates to and from the PC. ** /****************************************************************************** * Copyright (c) 2005 Nelson Henderson * * Permission to use, copy, modify, and distribute this software for any * purpose without fee is hereby granted, provided that this entire notice * is included in all copies of any software source which includes a copy of, * or any software source which is derived from modifications to this source code. * * THIS SOFTWARE IS BEING PROVIDED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED * WARRANTY. IN PARTICULAR, THE AUTHOR DOES NOT MAKE ANY REPRESENTATION OR * WARRANTY OF ANY KIND CONCERNING THE MERCHANTABILITY OF THIS SOFTWARE OR ITS * FITNESS FOR ANY PARTICULAR PURPOSE. * * Please report bugs or send useful modifications to: * Nelson Henderson email: nelson@inertialsolutions.us * *************************************************************************/ #include DARPA.H /*-----------External Functions Prototypes---------------------------------*/ extern void printf(const char *fmt, ...); extern void queue_rf_msg(unsigned char *bufptr, WORD msg_length); /*-----------Global Variable Declarations--------------------------------*/ unsigned short rflwrhead; /* write head index */ unsigned char rfch; /* Input char drives the Rx state. */ unsigned short rf_code; /* msg op code. */ unsigned short rfl_length; /* Total msg structure length in bytes including sync word and checksum */ unsigned short RFLst8; /*Current state of RFL Rx Queue unloading*/ unsigned short rflbuff_overflow; unsigned short rxed_csum; unsigned short rf_msg_err_count; /* errors for all msg types */ unsigned long rf_msg_count; /* count for all msg types */ /*--------------------------------------------------------------------------------------------------- Example of message formed and sent from the PC to the IS control computer. This MAN message sent by the PC will slew the servo's back and fourth. All message fields are Intel little Indian format. (LSP first) ---------------------------------------------------------------------------------------------------*/ void MAN_from_nav_computer(void) { unsigned char *bufptr = txmanbufptr; /* Base of union */ MANMSGst *strptr = txmanstrptr; /* struct ptr */ static WORD toggle; strptr->unique_word = 0x81FF; /* Word 1, Little Indian FF becomes LSP (sent out first from lowest buffer address) */ strptr->msgID = OPCODE_MANMSG; /* Word 2 */ strptr->msglength = SIZE_MANMSGst; /* Word 3 */ strptr->start_count = 0; /* Word 4,absolute wheel count to start command execution. */ /* 0 = Immediate override to current command execution. */ strptr->steer_channel = 0; /* Word 5, Servo output channel, 0 - 7 [0] */ strptr->exit_heading = 0; /* Word 8, Target heading on go straight, exit heading on turn. */ strptr->throttle_channel= 1; /* Word 9, Servo output channel, 0 - 7 [1] */ strptr->brake_channel = 2; /* Word 12, Servo output channel, 0 - 7 [2] */ strptr->ITC_channel = 0; /* Word 15, Input Transition Counter (ITC) channel #, 0 - 3 */ strptr->max_speed = 1000; /* Word 16, availiable to brake or throttle, 0= full stop */ if (toggle > 0) { /**** MESSAGES TO STEERING CHANNEL ***/ strptr->steer_angle = 0; /* Word 6, +/- steering angle(absolute deflection angle) .1 degrees.*/ strptr->steer_step_size =12; /* Word 7, Slew step size, 0 to 1048 */ /* 512 = Ignore roll out angle, Integer Units = .1 degrees*/ /**** MESSAGES TO THROTTLE CHANNEL ***/ strptr->throttle_pos = 0; /* Word 10, throttle angle(absolute deflection angle) 0 to 1048 */ strptr->throttle_step_size = 12; /* Word 11, Slew step size, 0 to 1048 */ /**** MESSAGES TO BRAKING CHANNEL ***/ strptr->brake_pos = 0; /* Word 13, braking severity(absolute deflection angle) 0 to 1048 */ strptr->brake_step_size =12; /* Word 14, Slew step size, 0 to 1048 */ strptr->transmission_set = 0; /* 0 = Neutral, 1 = first gear, 2 = second gear, 3 = 3rd gear, 4 = fourth gear, 5 = 5th gear -1 = reverse. */ toggle = 0; } else { /**** MESSAGES TO STEERING CHANNEL ***/ strptr->steer_angle = 1048; /* Word 6, +/- steering angle(absolute deflection angle) .1 degrees.*/ strptr->steer_step_size = 6; /* Word 7, Slew step size, 0 to 1048 */ /* 512 = Ignore roll out angle, Integer Units = .1 degrees*/ /**** MESSAGES TO THROTTLE CHANNEL ***/ strptr->throttle_pos = 1048; /* Word 10, throttle angle(absolute deflection angle) 0 to 1048 */ strptr->throttle_step_size = 6; /* Word 11, Slew step size, 0 to 1048 */ /**** MESSAGES TO BRAKING CHANNEL ***/ strptr->brake_pos = 1048; /* Word 13, braking severity(absolute deflection angle) 0 to 1048 */ strptr->brake_step_size = 6; /* Word 14, Slew step size, 0 to 1048 */ strptr->transmission_set = cvt_li16(3); /* 0 = Neutral, 1 = first gear, 2 = second gear, 3 = 3rd gear, 4 = fourth gear, 5 = 5th gear -1 = reverse. */ toggle = 1; } rfl_chksum_rx_msg (bufptr, SIZE_MANMSGst, 0); /* returns result in global calculated_rfl_checksum */ strptr->checksum = cvt_li16(calculated_rfl_checksum); queue_rf_msg(bufptr, SIZE_MANMSGst); /* Output the message bytes to the RFL Tx queue */ printf("\nMANMSG SteeringPos=%d Step=%d, ThrottlePos=%d Step=%d, BrakePos=%d Step=%d, TransSet=%d", strptr->steer_angle, strptr->steer_step_size, strptr->throttle_pos, strptr->throttle_step_size, strptr->brake_pos, strptr->brake_step_size, strptr->transmission_set ); } /*---------------------------------------------------------------------------------------------------*/ //void man_rpt(void) {} /* see example above */ void set_rpt(void) {} /* message stubs. */ void bit_rpt(void) {} void ode_rpt(void) {} void imu_rpt(void) {} /* Good 18 Sept 2003 */ void gps_rpt(void) {} /* Good 15 Sept 2003 */ void slo_rpt(void) {} void pid_rpt(void) {} void cmd_rpt(void) {} void eng_rpt(void) {} void mag_rpt(void) {} void rst_rpt(void) {} void key_rpt(void) {} void tty_rpt(void) {} void pwm_rpt(void) {} void rsc_rpt(void) {} void off_rpt(void) {} void ena_rpt(void) {} void uss_rpt(void) {} /*-----------External Buffer Definitions ------------------------------*/ extern Queue frRFQtab; /* Rx QTab */ /*-------------------------------------------------------------------------*/ void unused(void) {} void sss(void) {} void xtx(void) {} void msg_16(void){} void ignore(void){} void idle(void){} void mdmsg(void) { printf ("MisDirected Msg\n"); } /*---------------------------------------------------------------- ** Text strings for message dump ------------------------------------------------------------------*/ /* ** Message Identifier Strings */ static char UNUSED[] = "???"; static char SSS[] = "SSS"; static char RC8AE[] = "RC8AE"; static char RC8TR[] = "RC8TR"; static char RC6UCSD[] = "RC6UCSD"; static char RFLOAD[] = "RFLoad"; static char MANTXT[] = "MAN"; static char SETTXT[] = "SET"; static char BITTXT[] = "BIT"; static char ODETXT[] = "ODE"; static char IMUTXT[] = "IMU"; static char GPSTXT[] = "GPS"; static char SLOTXT[] = "SLO"; static char PIDTXT[] = "PID"; static char CMDTXT[] = "CMD"; static char ENGTXT[] = "ENG"; static char MAGTXT[] = "MAG"; static char RSTTXT[] = "RST"; static char KEYTXT[] = "KEY"; static char TTYTXT[] = "TTY"; static char PWMTXT[] = "PWM"; static char RSCTXT[] = "RSC"; static char OFFTXT[] = "OFF"; static char ENATXT[] = "ENA"; static char USSTXT[] = "USS"; /* ** Message handler lookup table */ struct msgitem msgtable[] = { UNUSED,0, (void *)mdmsg, /* 0 Unused opcode */ UNUSED,0, (void *)mdmsg, /* 1 */ UNUSED,0, (void *)mdmsg, /* 2 */ UNUSED,0, (void *)mdmsg, /* 3 */ UNUSED,0, (void *)mdmsg, /* 4 */ UNUSED,0, (void *)mdmsg, /* 5 */ UNUSED,0, (void *)mdmsg, /* 6 */ UNUSED,0, (void *)mdmsg, /* 7 */ UNUSED,0, (void *)mdmsg, /* 8 */ UNUSED,0, (void *)mdmsg, /* 9 */ UNUSED,0, (void *)mdmsg, /* 10 Unused opcode */ UNUSED,0, (void *)mdmsg, /* 11 */ UNUSED,0, (void *)mdmsg, /* 12 */ UNUSED,0, (void *)mdmsg, /* 13 */ UNUSED,0, (void *)mdmsg, /* 14 */ UNUSED,0, (void *)mdmsg, /* 15 */ UNUSED,0, (void *)mdmsg, /* 16 */ UNUSED,0, (void *)mdmsg, /* 17 */ UNUSED,0, (void *)mdmsg, /* 18 */ UNUSED,0, (void *)mdmsg, /* 19 */ MANTXT,0, (void *)man_rpt, /* 20 DARPA to IS*/ SETTXT,0, (void *)set_rpt, /* 21 to IS*/ BITTXT,0, (void *)bit_rpt, /* 22 to IS*/ ODETXT,0, (void *)ode_rpt, /* 23 to PC*/ IMUTXT,0, (void *)imu_rpt, /* 24 to PC*/ GPSTXT,0, (void *)gps_rpt, /* 25 to PC*/ SLOTXT,0, (void *)slo_rpt, /* 26 to PC*/ PIDTXT,0, (void *)pid_rpt, /* 27 to IS*/ CMDTXT,0, (void *)cmd_rpt, /* 28 */ ENGTXT,0, (void *)eng_rpt, /* 29 */ MAGTXT,0, (void *)mag_rpt, /* 30 to PC */ RSTTXT,0, (void *)rst_rpt, /* 31 to PC */ KEYTXT,0, (void *)key_rpt, /* 32 to IS */ TTYTXT,0, (void *)tty_rpt, /* 33 to PC */ PWMTXT,0, (void *)pwm_rpt, /* 34 to PC */ RSCTXT,0, (void *)rsc_rpt, /* 35 to PC */ OFFTXT,0, (void *)off_rpt, /* 36 to IS */ ENATXT,0, (void *)ena_rpt, /* 37 to IS */ USSTXT,0, (void *)uss_rpt, /* 38 to PC */ UNUSED,0, (void *)mdmsg, /* 39 */ UNUSED,0, (void *)mdmsg, /* 40 Unused opcode */ UNUSED,0, (void *)mdmsg, /* 41 */ UNUSED,0, (void *)mdmsg, /* 42 */ UNUSED,0, (void *)mdmsg, /* 43 */ UNUSED,0, (void *)mdmsg, /* 44 */ UNUSED,0, (void *)mdmsg, /* 45 */ UNUSED,0, (void *)mdmsg, /* 46 */ UNUSED,0, (void *)mdmsg, /* 47 */ UNUSED,0, (void *)mdmsg, /* 48 */ UNUSED,0, (void *)mdmsg, /* 49 */ // RC8AE, 0, (void *)rc8ae, /* 49 */ // RC8TR, 0, (void *)rc8tr, /* 50 Unused opcode */ UNUSED,0, (void *)mdmsg, /* 50 */ UNUSED,0, (void *)mdmsg, /* 51 */ UNUSED,0, (void *)mdmsg, /* 52 */ UNUSED,0, (void *)mdmsg, /* 53 */ UNUSED,0, (void *)mdmsg, /* 54 */ UNUSED,0, (void *)mdmsg, /* 55 */ UNUSED,0, (void *)mdmsg, /* 56 */ UNUSED,0, (void *)mdmsg, /* 57 */ UNUSED,0, (void *)mdmsg, /* 58 */ UNUSED,0, (void *)mdmsg, /* 59 */ UNUSED,0, (void *)mdmsg, /* 60 Unused opcode */ UNUSED,0, (void *)mdmsg, /* 61 */ UNUSED,0, (void *)mdmsg, /* 62 */ UNUSED,0, (void *)mdmsg, /* 63 */ UNUSED,0, (void *)mdmsg, /* 64 */ UNUSED,0, (void *)mdmsg, /* 65 */ UNUSED,0, (void *)mdmsg, /* 66 */ UNUSED,0, (void *)mdmsg, /* 67 */ UNUSED,0, (void *)mdmsg, /* 68 */ UNUSED,0, (void *)mdmsg, /* 69 */ UNUSED,0, (void *)mdmsg, /* 70 Unused opcode */ UNUSED,0, (void *)mdmsg, /* 71 */ UNUSED,0, (void *)mdmsg, /* 72 */ UNUSED,0, (void *)mdmsg, /* 73 */ UNUSED,0, (void *)mdmsg, /* 74 */ UNUSED,0, (void *)mdmsg, /* 75 */ UNUSED,0, (void *)mdmsg, /* 76 */ UNUSED,0, (void *)mdmsg, /* 77 */ UNUSED,0, (void *)mdmsg, /* 78 */ UNUSED,0, (void *)mdmsg, /* 79 */ UNUSED,0, (void *)mdmsg, /* 80 Unused opcode */ UNUSED,0, (void *)mdmsg, /* 81 */ UNUSED,0, (void *)mdmsg, /* 82 */ UNUSED,0, (void *)mdmsg, /* 83 */ UNUSED,0, (void *)mdmsg, /* 84 */ UNUSED,0, (void *)mdmsg, /* 85 */ UNUSED,0, (void *)mdmsg, /* 86 */ UNUSED,0, (void *)mdmsg, /* 87 */ UNUSED,0, (void *)mdmsg, /* 88 */ UNUSED,0, (void *)mdmsg, /* 89 */ UNUSED,0, (void *)mdmsg, /* 90 Unused opcode */ UNUSED,0, (void *)mdmsg, /* 91 */ UNUSED,0, (void *)mdmsg, /* 92 */ UNUSED,0, (void *)mdmsg, /* 93 */ UNUSED,0, (void *)mdmsg, /* 94 */ UNUSED,0, (void *)mdmsg, /* 95 */ UNUSED,0, (void *)mdmsg, /* 96 */ UNUSED,0, (void *)mdmsg, /* 97 */ UNUSED,0, (void *)mdmsg, /* 98 */ UNUSED,0, (void *)mdmsg, /* 99 */ UNUSED,0, (void *)mdmsg /* 100*/ }; /*------------------------------------------------------------------------------ DARPA Rx message storage allocation sharing a common input buffer. PC to IS sent anytime. /*------------------------------------------------------------------------------*/ union { char chr[RFLBUFZ]; /* rfl port msg buffer,206 max. */ BYTE buf[RFLBUFZ]; /* rfl port msg buffer,206 max. */ MANMSGst man; /* PC to IS sent anytime */ SETMSGst set; /* PC to IS sent anytime */ BITMSGst bit; /* PC to IS sent anytime */ PIDMSGst pid; /* PC to IS sent anytime */ CMDMSGst cmd; /* PC to IS sent anytime */ ENGMSGst eng; /* PC to IS sent anytime */ GPSMSGst gps; /* IS to PC sent anytime (Loop testing) */ ODEMSGst ode; /* IS to PC sent 8 per sec (Loop testing) */ IMUMSGst imu; /* IS to PC sent 8 per sec (Loop testing) */ SLOMSGst slo; /* IS to PC sent 8 seconds (Loop testing) */ MAGMSGst mag; /* IS to PC sent 8 per sec (Loop testing) */ RSTMSGst rst; /* IS to PC sent on reset (Loop testing) */ KEYMSGst key; /* PC to IS Key input to tport task */ TTYMSGst tty; /* IS to PC TTY output from tport task */ PWMMSGst pwm; /* IS to PC PWM servo report */ RSCMSGst rsc; /* IS to PC */ OFFMSGst off; /* PC to IS */ ENAMSGst ena; /* PC to IS */ USSMSGst uss; /* IS to PC */ } rxrfl; const MANMSGst *rxmanstrptr = &rxrfl.man; /* struct ptr */ const SETMSGst *rxsetstrptr = &rxrfl.set; /* struct ptr */ const BITMSGst *rxbitstrptr = &rxrfl.bit; /* struct ptr */ const PIDMSGst *rxpidstrptr = &rxrfl.pid; /* struct ptr */ const CMDMSGst *rxcmdstrptr = &rxrfl.cmd; /* struct ptr */ const ENGMSGst *rxengstrptr = &rxrfl.eng; /* struct ptr */ const GPSMSGst *rxgpsstrptr = &rxrfl.gps; /* struct ptr */ const ODEMSGst *rxodestrptr = &rxrfl.ode; /* struct ptr */ const IMUMSGst *rximustrptr = &rxrfl.imu; /* struct ptr */ const SLOMSGst *rxslostrptr = &rxrfl.slo; /* struct ptr */ const MAGMSGst *rxmagstrptr = &rxrfl.mag; /* struct ptr */ const RSTMSGst *rxrststrptr = &rxrfl.rst; /* struct ptr */ const KEYMSGst *rxkeystrptr = &rxrfl.key; /* struct ptr */ const TTYMSGst *rxttystrptr = &rxrfl.tty; /* struct ptr */ const PWMMSGst *rxpwmstrptr = &rxrfl.pwm; /* struct ptr */ const RSCMSGst *rxrscstrptr = &rxrfl.rsc; /* struct ptr */ const OFFMSGst *rxrofftrptr = &rxrfl.off; /* struct ptr */ const ENAMSGst *rxrenatrptr = &rxrfl.ena; /* struct ptr */ const USSMSGst *rxussstrptr = &rxrfl.uss; /* struct ptr */ /*------------------------------------------------------------------------------ Tx message buffers. Each message must have its own separate buffer because Real-time is forming multiple messages for Background to send. ------------------------------------------------------------------------------*/ #define SIZE_ODEMSGst sizeof(ODEMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent 8 times per second */ unsigned char buf[SIZE_ODEMSGst]; ODEMSGst ode; } txode; const unsigned char *odebufptr = &txode.buf[0]; /* Base of union */ const ODEMSGst *odestrptr = &txode.ode; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_IMUMSGst sizeof(IMUMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent 8 times per second */ unsigned char buf[SIZE_IMUMSGst]; IMUMSGst imu; } tximu; const unsigned char *imubufptr = &tximu.buf[0]; /* Base of union */ const IMUMSGst *imustrptr = &tximu.imu; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_GPSMSGst sizeof(GPSMSGst) /* returns size of type in bytes */ union { /* IS to PC once per second */ unsigned char buf[SIZE_GPSMSGst]; GPSMSGst gps; } txgps; const unsigned char *gpsbufptr = &txgps.buf[0]; /* Base of union */ const GPSMSGst *gpsstrptr = &txgps.gps; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_SLOMSGst sizeof(SLOMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent every 8 seconds */ unsigned char buf[SIZE_SLOMSGst]; SLOMSGst slo; } txslo; const unsigned char *slobufptr = &txslo.buf[0]; /* Base of union */ const SLOMSGst *slostrptr = &txslo.slo; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_MAGMSGst sizeof(MAGMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent every 8 seconds */ unsigned char buf[SIZE_MAGMSGst]; MAGMSGst mag; } txmag; const unsigned char *magbufptr = &txmag.buf[0]; /* Base of union */ const MAGMSGst *magstrptr = &txmag.mag; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_RSTMSGst sizeof(RSTMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent every 8 seconds */ unsigned char buf[SIZE_RSTMSGst]; RSTMSGst rst; } txrst; const unsigned char *rstbufptr = &txrst.buf[0]; /* Base of union */ const RSTMSGst *rststrptr = &txrst.rst; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_TTYMSGst sizeof(TTYMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent anytime */ unsigned char buf[SIZE_TTYMSGst]; char chr[SIZE_TTYMSGst]; TTYMSGst tty; } txtty; const unsigned char *ttybufptr = &txtty.buf[0]; /* Base of union */ const char *ttychrptr = &txtty.chr[0]+6;/* Start of string */ const TTYMSGst *ttystrptr = &txtty.tty; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_PWMMSGst sizeof(PWMMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent anytime */ unsigned char buf[SIZE_PWMMSGst]; PWMMSGst pwm; } txpwm; const unsigned char *pwmbufptr = &txpwm.buf[0]; /* Base of union */ const PWMMSGst *pwmstrptr = &txpwm.pwm; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_RSCMSGst sizeof(RSCMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent anytime */ unsigned char buf[SIZE_RSCMSGst]; RSCMSGst rsc; } txrsc; const unsigned char *rscbufptr = &txrsc.buf[0]; /* Base of union */ const RSCMSGst *rscstrptr = &txrsc.rsc; /* struct ptr */ /*------------------------------------------------------------------------------*/ #define SIZE_USSMSGst sizeof(USSMSGst) /* returns size of type in bytes */ union { /* IS to PC, sent anytime */ unsigned char buf[SIZE_USSMSGst]; USSMSGst uss; } txuss; const unsigned char *ussbufptr = &txuss.buf[0]; /* Base of union */ const USSMSGst *ussstrptr = &txuss.uss; /* struct ptr */ /*------------------------------------------------------------------------------ * Compute Checksum, Test Checksum *------------------------------------------------------------------------------*/ WORD calculated_rfl_checksum; BOOL rfl_chksum_rx_msg ( BYTE *buff, WORD len, WORD msgcs) { register WORD i; register WORD iv; register WORD cs = 0; /* Init checksum */ for (i=0; i RFLBUFZ) { RestartRFMsgSync(); /* start over */ rflbuff_overflow++; return; /* error exit. */ } (*RxRFLst8 [RFLst8]) (); /* index is current state */ } /* see end of listing for final code*/ /*--------------------------------------------------------------------------*/