void EIM_ResetTlmConnection(void)
{
int status = 0;
struct sockaddr_in si_tmp;
char cBuf[] = {1,0,0,0,0,0,0,0,0,0,0,0,0,0};
int len=14;
OS_printf("EIM: Sending reset message.\n");
memset((char *) &si_tmp, 0, sizeof(si_tmp));
si_tmp.sin_family = AF_INET;
inet_aton(EIM_WIFI_IP, &si_tmp.sin_addr);
si_tmp.sin_port = htons(EIM_NAV_DATA_PORT);
status = sendto(EIM_AppData.NavSocketID, cBuf, len, 0,
(struct sockaddr *) &si_tmp, sizeof(si_tmp) );
if ( status < 0 )
{
CFE_EVS_SendEvent(EIM_TELEM_TASK_ERR_EID, CFE_EVS_ERROR, "EIM: Failed to send NAVDATA reset. errno: %d", errno);
CFE_ES_ExitChildTask();
}
else
{
OS_printf("EIM: Reset message sent.\n");
}
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void CFE_ES_PerfLogDump(void){
int32 WriteStat;
uint32 i;
uint32 FileSize;
CFE_FS_Header_t FileHdr;
CFE_ES_RegisterChildTask();
/* Zero cFE header, then fill in fields */
CFE_PSP_MemSet(&FileHdr, 0, sizeof(CFE_FS_Header_t));
strcpy(&FileHdr.Description[0], CFE_ES_PERF_LOG_DESC);
FileHdr.SubType = CFE_FS_ES_PERFDATA_SUBTYPE;
/* write the cFE header to the file */
WriteStat = CFE_FS_WriteHeader( CFE_ES_PerfLogDumpStatus.DataFileDescriptor, &FileHdr);
if(WriteStat != sizeof(CFE_FS_Header_t))
{
CFE_ES_FileWriteByteCntErr(&CFE_ES_PerfLogDumpStatus.DataFileName[0],
sizeof(CFE_FS_Header_t),WriteStat);
OS_close(CFE_ES_PerfLogDumpStatus.DataFileDescriptor);
CFE_ES_ExitChildTask();
}/* end if */
FileSize = WriteStat;
/* write the performance metadata to the file */
WriteStat = OS_write(CFE_ES_PerfLogDumpStatus.DataFileDescriptor,(uint8 *)&Perf->MetaData,sizeof(CFE_ES_PerfMetaData_t));
if(WriteStat != sizeof(CFE_ES_PerfMetaData_t))
{
CFE_ES_FileWriteByteCntErr(&CFE_ES_PerfLogDumpStatus.DataFileName[0],
sizeof(CFE_ES_PerfMetaData_t),WriteStat);
OS_close(CFE_ES_PerfLogDumpStatus.DataFileDescriptor);
CFE_ES_ExitChildTask();
}/* end if */
FileSize += WriteStat;
CFE_ES_PerfLogDumpStatus.DataToWrite = Perf->MetaData.DataCount;
/* write the collected data to the file */
for(i=0; i < Perf->MetaData.DataCount; i++){
WriteStat = OS_write (CFE_ES_PerfLogDumpStatus.DataFileDescriptor, &Perf->DataBuffer[i], sizeof(CFE_ES_PerfDataEntry_t));
if(WriteStat != sizeof(CFE_ES_PerfDataEntry_t))
{
CFE_ES_FileWriteByteCntErr(&CFE_ES_PerfLogDumpStatus.DataFileName[0],
sizeof(CFE_ES_PerfDataEntry_t),WriteStat);
OS_close(CFE_ES_PerfLogDumpStatus.DataFileDescriptor);
/* Reset the DataToWrite variable, so a new file can be written */
CFE_ES_PerfLogDumpStatus.DataToWrite = 0;
CFE_ES_ExitChildTask();
}/* end if */
FileSize += WriteStat;
CFE_ES_PerfLogDumpStatus.DataToWrite--;
if((i % CFE_ES_PERF_ENTRIES_BTWN_DLYS) == 0){
OS_TaskDelay(CFE_ES_PERF_CHILD_MS_DELAY);
}/* end if */
}/* end for */
OS_close(CFE_ES_PerfLogDumpStatus.DataFileDescriptor);
CFE_EVS_SendEvent(CFE_ES_PERF_DATAWRITTEN_EID,CFE_EVS_DEBUG,
"%s written:Size=%d,EntryCount=%d",
&CFE_ES_PerfLogDumpStatus.DataFileName[0],FileSize,
Perf->MetaData.DataCount);
CFE_ES_ExitChildTask();
}/* end CFE_ES_PerfLogDump */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void CS_OneShotChildTask(void)
{
uint32 NewChecksumValue = 0;
int32 Status = -1; /* Init to OS error */
uint32 NumBytesRemainingCycles = 0;
uint32 NumBytesThisCycle = 0;
uint32 FirstAddrThisCycle = 0;
Status = CFE_ES_RegisterChildTask();
if (Status == CFE_SUCCESS)
{
NewChecksumValue = 0;
NumBytesRemainingCycles = CS_AppData.LastOneShotSize;
FirstAddrThisCycle = CS_AppData.LastOneShotAddress;
while (NumBytesRemainingCycles > 0)
{
NumBytesThisCycle = ( (CS_AppData.MaxBytesPerCycle < NumBytesRemainingCycles)
? CS_AppData.MaxBytesPerCycle
: NumBytesRemainingCycles);
NewChecksumValue = CFE_ES_CalculateCRC((void *) ((uint8*)FirstAddrThisCycle),
NumBytesThisCycle,
NewChecksumValue,
CS_DEFAULT_ALGORITHM);
/* Update the remainders for the next cycle */
FirstAddrThisCycle += NumBytesThisCycle;
NumBytesRemainingCycles -= NumBytesThisCycle;
OS_TaskDelay(CS_CHILD_TASK_DELAY);
}
/*Checksum Calculation is done! */
/* put the new checksum value in the baseline */
CS_AppData.LastOneShotChecksum = NewChecksumValue;
/* send event message */
CFE_EVS_SendEvent (CS_ONESHOT_FINISHED_INF_EID,
CFE_EVS_INFORMATION,
"OneShot checksum on Address: 0x%08X, size %d completed. Checksum = 0x%08X",
CS_AppData.LastOneShotAddress,
CS_AppData.LastOneShotSize,
CS_AppData.LastOneShotChecksum);
}/*end if register child task*/
else
{
/* Can't send event or write to syslog because this task isn't registered with the cFE. */
OS_printf("OneShot Child Task Registration failed!\n");
}
CS_AppData.ChildTaskInUse = FALSE;
CS_AppData.OneShotTaskInUse = FALSE;
CS_AppData.ChildTaskID = 0;
CFE_ES_ExitChildTask();
return;
}/* end CS_OneShotChildTask */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void CS_RecomputeTablesChildTask(void)
{
uint32 NewChecksumValue = 0;
CS_Res_Tables_Table_Entry_t * ResultsEntry = NULL;
uint16 PreviousState = CS_STATE_EMPTY;
boolean DoneWithEntry = FALSE;
int32 Status = -1; /* Init to OS error */
uint16 PreviousDefState = CS_STATE_EMPTY;
boolean DefEntryFound = FALSE;
uint16 DefEntryID = 0;
CS_Def_Tables_Table_Entry_t * DefTblPtr = NULL;
uint16 MaxDefEntries = 0;
CFE_TBL_Handle_t DefTblHandle = CFE_TBL_BAD_TABLE_HANDLE;
Status = CFE_ES_RegisterChildTask();
if (Status == CFE_SUCCESS)
{
/* Get the variables to use from the global data */
ResultsEntry = CS_AppData.RecomputeTablesEntryPtr;
/* we want to make sure that the entry isn't being checksummed in the
background at the same time we are recomputing */
PreviousState = ResultsEntry -> State;
ResultsEntry -> State = CS_STATE_DISABLED;
/* Set entry as if this is the first time we are computing the checksum,
since we want the entry to take on the new value */
ResultsEntry -> ByteOffset = 0;
ResultsEntry -> TempChecksumValue = 0;
ResultsEntry -> ComputedYet = FALSE;
/* Update the definition table entry as well. We need to determine which memory type is
being updated as well as which entry in the table is being updated. */
DefTblPtr = CS_AppData.DefTablesTblPtr;
MaxDefEntries = CS_MAX_NUM_TABLES_TABLE_ENTRIES;
DefTblHandle = CS_AppData.DefTablesTableHandle;
DefEntryID = 0;
while ((!DefEntryFound) && (DefEntryID < MaxDefEntries))
{
if ((strncmp(ResultsEntry->Name, DefTblPtr[DefEntryID].Name, CFE_TBL_MAX_FULL_NAME_LEN) == 0) &&
(DefTblPtr[DefEntryID].State != CS_STATE_EMPTY))
{
DefEntryFound = TRUE;
PreviousDefState = DefTblPtr[DefEntryID].State;
DefTblPtr[DefEntryID].State = CS_STATE_DISABLED;
CS_ResetTablesTblResultEntry(CS_AppData.TblResTablesTblPtr);
CFE_TBL_Modified(DefTblHandle);
}
else
{
DefEntryID++;
}
}
while(!DoneWithEntry)
{
Status = CS_ComputeTables(ResultsEntry, &NewChecksumValue, &DoneWithEntry);
if (Status == CS_ERR_NOT_FOUND)
{
break;
}
OS_TaskDelay(CS_CHILD_TASK_DELAY);
}
/* The new checksum value is stored in the table by the above functions */
if (Status == CS_ERR_NOT_FOUND)
{
CFE_EVS_SendEvent (CS_RECOMPUTE_ERROR_TABLES_ERR_EID,
CFE_EVS_ERROR,
"Table %s recompute failed. Could not get address",
ResultsEntry -> Name);
}
else
{
/* reset the entry's variables for a newly computed value */
ResultsEntry -> TempChecksumValue = 0;
ResultsEntry -> ByteOffset = 0;
ResultsEntry -> ComputedYet = TRUE;
/* send event message */
CFE_EVS_SendEvent (CS_RECOMPUTE_FINISH_TABLES_INF_EID,
CFE_EVS_INFORMATION,
"Table %s recompute finished. New baseline is 0x%08X",
ResultsEntry -> Name,
NewChecksumValue);
}
/* restore the entry's state */
ResultsEntry -> State = PreviousState;
/* Restore the definition table if we found one earlier */
if (DefEntryFound)
{
DefTblPtr[DefEntryID].State = PreviousDefState;
CS_ResetTablesTblResultEntry(CS_AppData.TblResTablesTblPtr);
CFE_TBL_Modified(DefTblHandle);
}
}/*end if register child task*/
else
{
/* Can't send event or write to syslog because this task isn't registered with the cFE. */
OS_printf("Recompute Tables Child Task Registration failed!\n");
}
CS_AppData.ChildTaskInUse = FALSE;
CFE_ES_ExitChildTask();
return;
}/* end CS_RecomputeTablesChildTask */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void CS_RecomputeEepromMemoryChildTask(void)
{
uint32 NewChecksumValue = 0;
char TableType[CS_TABLETYPE_NAME_SIZE];
CS_Res_EepromMemory_Table_Entry_t * ResultsEntry = NULL;
uint16 Table = 0;
uint16 EntryID = 0;
uint16 PreviousState = CS_STATE_EMPTY;
uint32 Status = -1; /* Init to OS error */
boolean DoneWithEntry = FALSE;
uint16 PreviousDefState = CS_STATE_EMPTY;
boolean DefEntryFound = FALSE;
uint16 DefEntryID = 0;
CS_Def_EepromMemory_Table_Entry_t * DefTblPtr = NULL;
uint16 MaxDefEntries = 0;
CFE_TBL_Handle_t DefTblHandle = CFE_TBL_BAD_TABLE_HANDLE;
CS_Res_Tables_Table_Entry_t * TablesTblResultEntry = NULL;
Status = CFE_ES_RegisterChildTask();
strncpy(TableType, "Undef Tbl", CS_TABLETYPE_NAME_SIZE); /* Initialize table type string */
if (Status == CFE_SUCCESS)
{
Table = CS_AppData.ChildTaskTable;
EntryID = CS_AppData.ChildTaskEntryID;
ResultsEntry = CS_AppData.RecomputeEepromMemoryEntryPtr;
/* we want to make sure that the entry isn't being checksummed in the
background at the same time we are recomputing */
PreviousState = ResultsEntry -> State;
ResultsEntry -> State = CS_STATE_DISABLED;
/* Set entry as if this is the first time we are computing the checksum,
since we want the entry to take on the new value */
ResultsEntry -> ByteOffset = 0;
ResultsEntry -> TempChecksumValue = 0;
ResultsEntry -> ComputedYet = FALSE;
/* Update the definition table entry as well. We need to determine which memory type is
being updated as well as which entry in the table is being updated. */
if ((Table != CS_OSCORE) && (Table != CS_CFECORE))
{
if (Table == CS_EEPROM_TABLE)
{
DefTblPtr = CS_AppData.DefEepromTblPtr;
MaxDefEntries = CS_MAX_NUM_EEPROM_TABLE_ENTRIES;
DefTblHandle = CS_AppData.DefEepromTableHandle;
TablesTblResultEntry = CS_AppData.EepResTablesTblPtr;
}
else
{
DefTblPtr = CS_AppData.DefMemoryTblPtr;
MaxDefEntries = CS_MAX_NUM_MEMORY_TABLE_ENTRIES;
DefTblHandle = CS_AppData.DefMemoryTableHandle;
TablesTblResultEntry = CS_AppData.MemResTablesTblPtr;
}
if (EntryID < MaxDefEntries)
{
/* This assumes that the definition table entries are in the same order as the
results table entries, which should be a safe assumption. */
if ((ResultsEntry->StartAddress == DefTblPtr[EntryID].StartAddress) &&
(DefTblPtr[EntryID].State != CS_STATE_EMPTY))
{
DefEntryFound = TRUE;
PreviousDefState = DefTblPtr[EntryID].State;
DefTblPtr[EntryID].State = CS_STATE_DISABLED;
DefEntryID = EntryID;
CS_ResetTablesTblResultEntry(TablesTblResultEntry);
CFE_TBL_Modified(DefTblHandle);
}
}
}
while(!DoneWithEntry)
{
CS_ComputeEepromMemory(ResultsEntry, &NewChecksumValue, &DoneWithEntry);
OS_TaskDelay(CS_CHILD_TASK_DELAY);
}
/* The new checksum value is stored in the table by the above functions */
/* reset the entry's variables for a newly computed value */
ResultsEntry -> TempChecksumValue = 0;
ResultsEntry -> ByteOffset = 0;
ResultsEntry -> ComputedYet = TRUE;
/* restore the entry's previous state */
ResultsEntry -> State = PreviousState;
/* Restore the definition table if we found one earlier */
if (DefEntryFound)
{
DefTblPtr[DefEntryID].State = PreviousDefState;
CS_ResetTablesTblResultEntry(TablesTblResultEntry);
CFE_TBL_Modified(DefTblHandle);
}
/* send event message */
if( Table == CS_EEPROM_TABLE)
{
strncpy(TableType, "Eeprom", CS_TABLETYPE_NAME_SIZE);
}
if( Table == CS_MEMORY_TABLE)
{
strncpy(TableType, "Memory", CS_TABLETYPE_NAME_SIZE);
}
if( Table == CS_CFECORE)
{
strncpy(TableType, "cFE Core", CS_TABLETYPE_NAME_SIZE);
CS_AppData.CfeCoreBaseline = NewChecksumValue;
}
if( Table == CS_OSCORE)
{
strncpy(TableType, "OS", CS_TABLETYPE_NAME_SIZE);
CS_AppData.OSBaseline = NewChecksumValue;
}
CFE_EVS_SendEvent (CS_RECOMPUTE_FINISH_EEPROM_MEMORY_INF_EID,
CFE_EVS_INFORMATION,
"%s entry %d recompute finished. New baseline is 0X%08X",
TableType, EntryID, NewChecksumValue);
}/* end if child task register */
else
{
/* Can't send event or write to syslog because this task isn't registered with the cFE. */
OS_printf("Recompute for Eeprom or Memory Child Task Registration failed!\n");
}
CS_AppData.ChildTaskInUse = FALSE;
CFE_ES_ExitChildTask();
return;
}/* end CS_RecomputeEepromMemoryChildTask */
void EIM_NavDataTlmMain(void)
{
int32 Status = CFE_SUCCESS;
struct sockaddr_in addrNav;
uint32 RunStatus = CFE_ES_APP_RUN;
Status = CFE_ES_RegisterChildTask();
if(Status != CFE_SUCCESS)
{
CFE_EVS_SendEvent(EIM_TELEM_TASK_ERR_EID, CFE_EVS_ERROR,
"EIM: Failed to register telemetry listener task.");
CFE_ES_ExitChildTask();
}
/*
** Install the delete handler
*/
OS_TaskInstallDeleteHandler((void *)(&EIM_NavDataTlm_delete_callback));
/* Create sockets */
EIM_AppData.NavSocketID = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if(EIM_AppData.NavSocketID < 0)
{
CFE_EVS_SendEvent(EIM_TELEM_TASK_ERR_EID, CFE_EVS_ERROR,"EIM: Create Nav telemetry socket failed = %d", errno);
CFE_ES_ExitChildTask();
}
//EIM_AppData.ATCmdSocketID = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
//if(EIM_AppData.ATCmdSocketID < 0)
//{
// CFE_EVS_SendEvent(EIM_TELEM_TASK_ERR_EID, CFE_EVS_ERROR,"EIM: Create Nav command socket failed = %d", errno);
// CFE_ES_ExitChildTask();
//}
memset((char *) &addrNav, 0, sizeof(addrNav));
addrNav.sin_family = AF_INET;
addrNav.sin_port = htons(EIM_NAV_DATA_PORT+100);
addrNav.sin_addr.s_addr = htonl(INADDR_ANY);
OS_printf("EIM: Binding NAV socket.\n");
if ( bind(EIM_AppData.NavSocketID, (struct sockaddr *) &addrNav,
sizeof(addrNav)) < 0)
{
CFE_EVS_SendEvent(EIM_TELEM_TASK_ERR_EID, CFE_EVS_ERROR,
"EIM: Failed to bind NAV command socket. errno: %d", errno);
CFE_ES_ExitChildTask();
}
/* Set addresses */
//bzero((char *) &EIM_AppData.CmdSocketAddress, sizeof(EIM_AppData.CmdSocketAddress));
//EIM_AppData.CmdSocketAddress.sin_family = AF_INET;
//EIM_AppData.CmdSocketAddress.sin_addr.s_addr = inet_addr("0.0.0.0");
//EIM_AppData.CmdSocketAddress.sin_port = htons(0);
//bzero((char *) &EIM_AppData.CmdARDroneAddress, sizeof(EIM_AppData.CmdARDroneAddress));
//EIM_AppData.CmdARDroneAddress.sin_family = AF_INET;
//EIM_AppData.CmdARDroneAddress.sin_addr.s_addr = inet_addr(EIM_WIFI_IP);
//EIM_AppData.CmdARDroneAddress.sin_port = htons(EIM_AT_PORT);
//if ( bind(EIM_AppData.ATCmdSocketID,
// (struct sockaddr *) &EIM_AppData.CmdSocketAddress,
// sizeof(EIM_AppData.CmdSocketAddress)) < 0)
//{
// CFE_EVS_SendEvent(EIM_TELEM_TASK_ERR_EID, CFE_EVS_ERROR,
// "EIM: Failed to bind command socket. errno: %d", errno);
// CFE_ES_ExitChildTask();
//}
//
//EIM_InitATCmdSockets();
OS_TaskDelay(100);
EIM_ResetTlmConnection();
if(CFE_ES_RunLoop(&RunStatus) == FALSE)
{
EIM_AppData.Hk.NavDataState = EIM_NAVDATA_FAILED;
}
else
{
EIM_AppData.Hk.NavDataState = EIM_NAVDATA_ENABLED;
}
while (CFE_ES_RunLoop(&RunStatus) == TRUE)
{
EIM_ReadTelem();
if(EIM_AppData.Hk.ARDroneState.CommWatchdog == TRUE)
{
if(EIM_AppData.Hk.ARDrone2TimedOut == FALSE)
{
CFE_EVS_SendEvent(EIM_TELEM_TASK_ERR_EID, CFE_EVS_ERROR, "EIM: ARDrone timedout. Resetting watchdog.");
EIM_AppData.Hk.ARDrone2TimedOut = TRUE;
}
EIM_cmdComWdg();
}
else
{
if(EIM_AppData.Hk.ARDrone2TimedOut == TRUE)
{
CFE_EVS_SendEvent(EIM_TELEM_TASK_ERR_EID, CFE_EVS_ERROR, "EIM: ARDrone watchdog reset.");
EIM_AppData.Hk.ARDrone2TimedOut = FALSE;
}
}
}
close(EIM_AppData.ATCmdSocketID);
close(EIM_AppData.NavSocketID);
EIM_AppData.ATCmdSocketID = 0;
EIM_AppData.NavSocketID = 0;
EIM_AppData.Hk.NavDataState = EIM_NAVDATA_DISABLED;
CFE_ES_ExitApp(RunStatus);
}
