/**************************************************************************//**
* @brief Send a packet to the board controller.
*
* @param[in] pkt Pointer to a @ref BCP_Packet instance.
*
* @return @ref BSP_STATUS_OK.
*****************************************************************************/
int BSP_BccPacketSend( BCP_Packet *pkt )
{
int i;
/* Apply magic */
pkt->magic = BSP_BCP_MAGIC;
/* Transmit packet magic */
TxByte( pkt->magic );
/* Transmit packet type */
TxByte( pkt->type );
/* Transmit packet length */
TxByte( pkt->payloadLength );
#if ( BSP_BCP_VERSION == 2 )
/* Transmit reserved byte */
TxByte( pkt->reserved );
#endif
/* Transmit packet payload */
for ( i=0; i<pkt->payloadLength; i++ )
{
TxByte( pkt->data[i] );
}
return BSP_STATUS_OK;
}
void SendAttitude(void)
{
static byte i;
TxByte(byte('$')); // sentinel not included in checksum
TxCheckSum = 0;
TxByte(UAVXRazorPacketTag);
TxByte(23);
// angles in milliradian
TxWord((int)(Roll * 1000.0 ));
TxWord((int)(Pitch * 1000.0 ));
TxWord((int)(Yaw * 1000.0 ));
TxWord((int)(Omega_V[0] * 1000.0 ));
TxWord((int)(Omega_V[1] * 1000.0 ));
TxWord((int)(Omega_V[2] * 1000.0 ));
TxWord((int)(Acc_V[0] * AccToMilliG));
TxWord((int)(Acc_V[1] * AccToMilliG));
TxWord((int)((Acc_V[2] - GRAVITY) * AccToMilliG));
TxWord((int)(MagHeading * 1000.0));
TxByte(TxCheckSum);
#if PRINT_UAVX_READABLE == 1
Serial.println();
#endif // PRINT_UAVX_READABLE
} // SendAttitude
/******************************************************************************
Name: _write
Description: this function replaces stubbing function of C-Lib _write for
writing to terminal, OSD, memory.
Parameters: 1) buffer to be printed.
2) number of bytes to be printed.
Returns: number of bytes
******************************************************************************/
int _write(int handle, const char *buffer, unsigned int length) {
unsigned int i;
for(i=0;i<length;i++)
{
if(*buffer=='\n') TxByte('\r'); /* do automatic carriage return */
TxByte(*buffer++);
}
return(length);
}
int _write( int file, char *ptr, int len )
{
unsigned int index;
for( index=0; index < len; index++ )
{
if( *ptr == '\n' ) TxByte( '\r' ); /* do automatic carriage return */
TxByte( *ptr++ );
}
return len;
}
void Revolver::TxStr(char *tx, int len)
{
unsigned char cksm = 0, ch;
int c;
TxByte('$');
ch = ByteHex((m_Addr >> 4) & 0x0F);
TxByte(ch);
cksm += ch;
ch = ByteHex(m_Addr & 0x0F);
TxByte(ch);
cksm += ch;
for(c = 0; c < len; c++)
{
TxByte(tx[c]);
cksm += tx[c];
}
TxByte('#');
ch = ByteHex((cksm >> 4) & 0x0F);
TxByte(ch);
ch = ByteHex(cksm & 0x0F);
TxByte(ch);
TxByte(13);
}
void GHA_BmIntHand(U8 Channel, U32 Bm1Or2)
{
// UT_PrStr((CSTR*)"=>BmInt:"); UT_PrHex(Bm1Or2,1); TxByte(',');UT_PrHex(Channel,2);
#ifdef DEBMSG
TxByte('~');
#endif
XD_DmxSiIntHand(Bm1Or2,Channel);
}
/******************************************************************************
simple print functions
if you don't want to use the almighty printf() function because of memory
limitations.
******************************************************************************/
void GM_PrStr(const char *strP)
{
U8 ch;
ch = *strP++;
while (ch)
{
if (ch==3) /* if Eddie's special ESC */
{
ch = *strP++;
if (!ch) return;
}
else
{
TxByte(ch);
if (ch==10) TxByte(13);
ch = *strP++;/*Gordon add here*/
}
}
}
void GM_PrintChar(char cx)
{
TxByte(cx);
}
HDI_STATUS HDIRequestSectionAcquisitionXt (DEVICE_NO device_no,
SUB_TABLE_PARAMS *sub_table_filter_params_p,
ACQUISITION_PARAMS *acquisition_filter_params_p,
USHORT event_qualifier,
TAG request_tag,
TAG *acquisition_tag_p)
{
T_STREAM_FILTRER_DATA *pFILTS = NULL;
T_STREAM_FILT_ALLOC_INFO *pSAD;
int AllocStrmIdx, ix;
OpU16 ActPidVal;
int FiltBankIdx, FiltIdx, RunningStream;
// LastCancelTag = 0; // clear global
AllocStrmIdx = 0;
if ( (!sub_table_filter_params_p)||(!acquisition_filter_params_p)||(!acquisition_tag_p)
||(sub_table_filter_params_p->sub_table_id_mask > EXTENSIONS_123)
||(acquisition_filter_params_p->acquisition_mask > ACQUIRE_SECTIONS_VERSION) ) {
// TxByte('e');
return (PARAMETER_ERROR);
}
/*
if ((acquisition_filter_params_p->first_section_number==0xFF)&&(acquisition_filter_params_p->last_section_number==0xFF) ) {
SYS_DEBUG_INFO(DEBL3_NSTR, (CSTR*)"x****** Special Test ******", 0,0,0); RxByte();
}
*/
ActPidVal = acquisition_filter_params_p->pid;
SYS_DEBUG_INFO(DEBL3_NSTR,(CSTR*)"4\n##Request Pid:", ActPidVal, (CSTR*)"8 rT:",(U32)request_tag);
SYS_DEBUG_INFO(DEBL3_STR, (CSTR*)"2>ReqAcqu: AcqMask:", acquisition_filter_params_p->acquisition_mask, (CSTR*)"2 SubTabIdMask:", sub_table_filter_params_p->sub_table_id_mask);
#if SOME_DEBUG
DebSystemMessage(DEBL3_NSTR,(CSTR*)"4#ReqP:", ActPidVal, (CSTR*)"5 rT:",(U32)request_tag);
DebSystemMessage(DEBL3_STR, (CSTR*)"2 Msk:", acquisition_filter_params_p->acquisition_mask, (CSTR*)"2 STM:", sub_table_filter_params_p->sub_table_id_mask);
#endif
#ifdef PAT_DEBUG
if (ActPidVal==0x0000) { // if PAT
DebSystemMessage(DEBL3_NSTR,(CSTR*)"4->rqPat:", ActPidVal, (CSTR*)"5 rT:",(U32)request_tag);
// DebSystemMessage(DEBL3_STR, (CSTR*)"2 Msk:", acquisition_filter_params_p->acquisition_mask, (CSTR*)"2 STM:", sub_table_filter_params_p->sub_table_id_mask);
}
#endif
//-------------------------------------------------------------------
//--- check if a Stream with the Aqu.Param.PID is already running ---
//-------------------------------------------------------------------
RunningStream = 0; // Def.Flag: a stream with this PID does not exist
pSAD = GHA_StrmsAllocInfo; // pointer to Sreams-Alloc-Info
for (FiltBankIdx=0;FiltBankIdx<MAX_SECTION_STREAMS;FiltBankIdx++) {
// DebSystemMessage(DEBL3_STR, (CSTR*)"4~", pSAD->FiltBankStrmPid, 0, 0);
if (pSAD->FiltBankStrmPid == ActPidVal) { // if this PID is found
AllocStrmIdx = pSAD->FhaAllocIdx; // this is the SI-Channel number
pFILTS = &StrmsFiltData[FiltBankIdx * MAX_FILT_PER_STREAM]; // pointer to FiltersData of SamePID
SYS_DEBUG_INFO(DEBL3_NSTR, (CSTR*)"1>Pid-stream already running, FiltBankIdx:", FiltBankIdx, (CSTR*)"8 FB:", (U32)pFILTS);
#if SOME_DEBUG
DebSystemMessage(DEBL3_NSTR, (CSTR*)"1>Pid-stream already running, FiltBankIdx:", FiltBankIdx, (CSTR*)"8 FB:", (U32)pFILTS);
#endif
RunningStream = 1;
break;
}
pSAD++;
}
//--- find free FilterBank for new PID-Stream ---
if (!RunningStream) {
// TxByte('n');
SYS_DEBUG_INFO(DEBL3_NSTR, (CSTR*)"x <- new stream", 0,0,0);
AllocStrmIdx = GHA_AllocateSiStream(0); // Allocate an SI-Stream
if (!AllocStrmIdx) {
return (NO_SECTION_FILTER);
}
// TxByte('A'-1+AllocStrmIdx);
pSAD = GHA_StrmsAllocInfo; // init
for (FiltBankIdx=0;FiltBankIdx<MAX_SECTION_STREAMS;FiltBankIdx++) { // find place in "AllocInfoTable"
// DebSystemMessage(DEBL3_STR, (CSTR*)"4k", pSAD->FiltBankStrmPid, 0,0);
if (pSAD->FiltBankStrmPid == NOT_A_PID) { // if this FilerBank not used yet
break;
}
// TxByte('n');
pSAD++;
}
if (FiltBankIdx>=MAX_SECTION_STREAMS) {
TxByte('z');
SYS_DEBUG_INFO(DEBL3_STR, (CSTR*)"x{No FitlerBank available}", 0,0,0);
return (NO_SECTION_FILTER);
}
pFILTS = &StrmsFiltData[FiltBankIdx*MAX_FILT_PER_STREAM]; // pointer to FilterData of SamePID
pSAD->FiltBankStrmPid = ActPidVal; // store PID for this filter
pSAD->FhaAllocIdx = AllocStrmIdx; // store the Stream-Index Handle
pSAD->NFiltersActive = 0; // just make sure
// DebSystemMessage(DEBL3_NSTR, (CSTR*)"8 &", (U32)pSAD, (CSTR*)"8 $", (U32) &GHA_StrmsAllocInfo);
// DebSystemMessage(DEBL3_STR, (CSTR*)"2 x", FiltBankIdx, (CSTR*)"4}",GHA_StrmsAllocInfo[0].FiltBankStrmPid);
}
SYS_DEBUG_INFO(DEBL3_STR, (CSTR*)"1 FBx:", FiltBankIdx, (CSTR*)"1 nFA:", pSAD->NFiltersActive);
//--- now find a free filter inside the filter-bank ----
// SYS_DEBUG_INFO(DEBL3_STR, (CSTR*)"6 >Alloc SiFilt @", (U32)pFILTS,0,0);
for (FiltIdx=0;FiltIdx<MAX_FILT_PER_STREAM;FiltIdx++) {
if (!pFILTS->Hdi_ReqTag) { // if this filter is not allocated
break;
}
pFILTS++;
}
if (FiltIdx>=MAX_FILT_PER_STREAM) {
TxByte('h');
SYS_DEBUG_INFO(DEBL3_STR, (CSTR*)"x{No Fitler in Bank available}", 0,0,0);
return (NO_SECTION_FILTER);
}
SYS_DEBUG_INFO(DEBL3_STR, (CSTR*)"1 ix:", FiltIdx, 0,0);
pFILTS->AcqParams = *acquisition_filter_params_p; // copy Acqu.Parameters
pFILTS->SubTabFltP = *sub_table_filter_params_p; // copy SubTabFilt Parameters
pFILTS->Hdi_ReqTag = request_tag;
pFILTS->Hdi_EventQual = event_qualifier;
*acquisition_tag_p = (TAG) pFILTS; // save pointer to the filter data
SYS_DEBUG_INFO(DEBL3_STR, (CSTR*)"2>FiIx:", FiltIdx, (CSTR*)"6 TAG@", (U32)pFILTS);
// TxByte('f');TxByte('0'+FiltIdx);
//--------------------------------
//--- Update the SectionFilter ---
//--------------------------------
ComposeSectionFilterData(acquisition_filter_params_p, sub_table_filter_params_p);
// GHA_StopSiStream(AllocStrmIdx, 0); // just for Test ?
GHA_UpdateSiFilterParameters(AllocStrmIdx, pSAD->NFiltersActive, ActFilterTableId, ActFilterSectionBytes);
pSAD->NFiltersActive++; // inc. number of used filters
if (!RunningStream) {
GHA_StartSiStream(AllocStrmIdx, ActPidVal);
}
// DebSystemMessage(DEBL3_STR, (CSTR*)"4 CkStmPid",GHA_StrmsAllocInfo[0].FiltBankStrmPid, 0,0);
// TxByte('o');
return (OK);
}
void TxWord(int w)
{
TxByte(lowByte(w));
TxByte(highByte(w));
} // TxWord
