void SIMCI_ReportHousekeeping(void) {
SIMCI_AppData.HkPacket.uplink_socket_connected = SIMCI_AppData.UplinkSocketConnected;
SIMCI_AppData.HkPacket.ingest_socket_connected = SIMCI_AppData.IngestSocketConnected;
CFE_SB_TimeStampMsg((CFE_SB_MsgPtr_t)&SIMCI_AppData.HkPacket);
CFE_SB_SendMsg((CFE_SB_MsgPtr_t)&SIMCI_AppData.HkPacket);
}
/******************************************************************************
** Function: LABTLM_SendHousekeepingPkt
**
*/
void LABTLM_SendHousekeepingPkt(void)
{
/*
** LABTLM Data
*/
LabTlmHkPkt.ValidCmdCnt = LabTlm.CmdMgr.ValidCmdCnt;
LabTlmHkPkt.InvalidCmdCnt = LabTlm.CmdMgr.InvalidCmdCnt;
/*
** TBLMGR Data
*/
LabTlmHkPkt.PktTblLoadActive = LabTlm.TblMgr.PktTbl.LoadActive;
LabTlmHkPkt.PktTblLastLoadValid = LabTlm.TblMgr.PktTbl.LastLoadValid;
LabTlmHkPkt.PktTblAttrErrCnt = LabTlm.TblMgr.PktTbl.AttrErrCnt;
/*
** PKTMGR Data
** - At a minimum all pktmgr variables effected by a reset must be included
** - Some of these may be more diagnostic but not enough to warrant a
** separate diagnostic. Also easier for the user not to hhave to command it.
*/
LabTlmHkPkt.TlmSockId = (uint16)LabTlm.PktMgr.TlmSockId;
strncpy(LabTlmHkPkt.TlmDestIp, LabTlm.PktMgr.TlmDestIp, PKTMGR_IP_STR_LEN);
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &LabTlmHkPkt);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &LabTlmHkPkt);
} /* End LABTLM_SendHousekeepingPkt() */
void TT_HousekeepingCmd(CFE_SB_MsgPtr_t msg)
{
uint16 ExpectedLength = sizeof(TT_NoArgsCmd_t);
/*
** Verify command packet length...
*/
if (TT_VerifyCmdLength(msg, ExpectedLength))
{
/*
** Get command execution counters...
*/
TT4_AppData.HkPacket.CmdCount = TT4_AppData.CmdCount;
TT4_AppData.HkPacket.ErrCount = TT4_AppData.ErrCount;
/*
** Send housekeeping telemetry packet...
*/
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &TT4_AppData.HkPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &TT4_AppData.HkPacket);
/*
** This command does not affect the command execution counter...
*/
}
return;
} /* End of TT_HousekeepingCmd() */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void $APPLICATION_NAME$_ReportHousekeeping(void)
{
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &$APPLICATION_NAME$_HkTelemetryPkt);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &$APPLICATION_NAME$_HkTelemetryPkt);
return;
} /* End of $APPLICATION_NAME$_ReportHousekeeping() */
void CFE_TIME_HousekeepingCmd(void)
{
CFE_TIME_Reference_t Reference;
/*
** Get reference time values (local time, time at tone, etc.)...
*/
CFE_TIME_GetReference(&Reference);
/*
** Update TIME portion of Critical Data Store...
*/
CFE_TIME_UpdateResetVars(&Reference);
/*
** Collect housekeeping data from Time Services utilities...
*/
CFE_TIME_GetHkData(&Reference);
/*
** Send housekeeping telemetry packet...
*/
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &CFE_TIME_TaskData.HkPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &CFE_TIME_TaskData.HkPacket);
/*
** Note: we only increment the command execution counter when
** processing CFE_TIME_CMD_MID commands...
*/
return;
} /* End of CFE_TIME_HousekeepingCmd() */
void TO_OutputStatus(void)
{
/* Timestamp Message */
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &TO_AppData.HkPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &TO_AppData.HkPacket);
}
void CI_ReportHousekeeping(void)
{
CI_HkTelemetryPkt.SocketConnected = CI_SocketConnected;
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &CI_HkTelemetryPkt);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &CI_HkTelemetryPkt);
return;
} /* End of CI_ReportHousekeeping() */
void THRSIM_GetValveStates(void) {
/* Timestamp and send out our message */
if (THRSIM_ReadDeviceData(&THRSIM_AppData.thr_sim_msg) != CFE_SUCCESS) {
CFE_EVS_SendEvent(THRSIM_DEVICE_ERR_EID, CFE_EVS_ERROR, "Error reading from valves device");
CFE_PSP_MemSet(CFE_SB_GetUserData((CFE_SB_MsgPtr_t)&THRSIM_AppData.thr_sim_msg),
0,
CFE_SB_GetUserDataLength((CFE_SB_MsgPtr_t)&THRSIM_AppData.thr_sim_msg));
}
CFE_SB_TimeStampMsg((CFE_SB_MsgPtr_t) &THRSIM_AppData.thr_sim_msg);
CFE_SB_SendMsg((CFE_SB_MsgPtr_t) &THRSIM_AppData.thr_sim_msg);
}
boolean LABTLM_SendDataTypeTlmCmd(const CFE_SB_MsgPtr_t MsgPtr)
{
int32 Status;
CFE_SB_TimeStampMsg((CFE_SB_MsgPtr_t) &ToDataTypePkt);
Status = CFE_SB_SendMsg((CFE_SB_Msg_t *)&ToDataTypePkt);
return (Status == CFE_SUCCESS);
} /* End LABTLM_SendDataTypeTlmCmd() */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void TO_output_data_types_packet(void)
{
int16 i;
char string_variable[10] = "ABCDEFGHIJ";
/* initialize data types packet */
CFE_SB_InitMsg(&data_types_pkt,
TO_LAB_DATA_TYPES_MID,
sizeof(data_types_pkt), TRUE);
CFE_SB_TimeStampMsg((CFE_SB_MsgPtr_t) &data_types_pkt);
/* initialize the packet data */
data_types_pkt.synch = 0x6969;
#if 0
data_types_pkt.bit1 = 1;
data_types_pkt.bit2 = 0;
data_types_pkt.bit34 = 2;
data_types_pkt.bit56 = 3;
data_types_pkt.bit78 = 1;
data_types_pkt.nibble1 = 0xA;
data_types_pkt.nibble2 = 0x4;
#endif
data_types_pkt.bl1 = FALSE;
data_types_pkt.bl2 = TRUE;
data_types_pkt.b1 = 16;
data_types_pkt.b2 = 127;
data_types_pkt.b3 = 0x7F;
data_types_pkt.b4 = 0x45;
data_types_pkt.w1 = 0x2468;
data_types_pkt.w2 = 0x7FFF;
data_types_pkt.dw1 = 0x12345678;
data_types_pkt.dw2 = 0x87654321;
data_types_pkt.f1 = 90.01;
data_types_pkt.f2 = .0000045;
data_types_pkt.df1 = 99.9;
data_types_pkt.df2 = .4444;
for (i=0; i < 10; i++) data_types_pkt.str[i] = string_variable[i];
CFE_SB_SendMsg((CFE_SB_Msg_t *)&data_types_pkt);
} /* End of TO_output_data_types_packet() */
void CFE_TIME_DiagCmd(void)
{
CFE_TIME_TaskData.CmdCounter++;
/*
** Collect housekeeping data from Time Services utilities...
*/
CFE_TIME_GetDiagData();
/*
** Send housekeeping telemetry packet...
*/
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &CFE_TIME_TaskData.DiagPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &CFE_TIME_TaskData.DiagPacket);
CFE_EVS_SendEvent(CFE_TIME_DIAG_EID, CFE_EVS_DEBUG,
"Request diagnostics command");
return;
} /* End of CFE_TIME_DiagCmd() */
/*
** Function Prologue
**
** Function Name: EVS_SendEventInternal
** from: cfe_evs_utils.c in cFE 3.1, originally named EVS_SendEvent
**
** Purpose: This routine sends an EVS event message out the software bus and all
** enabled output ports if the calling application has been determined to
** be registered and the event message is unfiltered
**
** Assumptions and Notes:
*/
int32 EVS_SendEventInternal(uint32 AppID, CFE_TIME_SysTime_t Time, uint16 EventID, uint16 EventType, char *EventString)
{
int32 Status = CFE_SUCCESS;
CFE_EVS_Packet_t EVS_Packet;
boolean IsFiltered = FALSE;
if(Status == CFE_SUCCESS)
{
if (!(IsFiltered))
{
/* Initialize CCSDS event packet */
CFE_SB_InitMsg(&(EVS_Packet), CFE_EVS_EVENT_MSG_MID, sizeof(CFE_EVS_Packet_t), FALSE);
/* Set the packet timestamp */
CFE_SB_SetMsgTime((CFE_SB_Msg_t *) &EVS_Packet, Time);
/* Initialize event message string */
EVS_Packet.Message[0] = '\0';
/* Obtain task and system information */
/* EVS_Packet.PacketID.SpacecraftID = OS_BSPGetSpacecraftId(); */
CFE_ES_GetAppName(EVS_Packet.PacketID.AppName, AppID, OS_MAX_API_NAME);
/* EVS_Packet.PacketID.ProcessorID = OS_BSPGetProcessorId(); */
EVS_Packet.PacketID.EventID = EventID;
EVS_Packet.PacketID.EventType = EventType;
/* Copy message string to event packet message buffer */
strncpy(EVS_Packet.Message, EventString, CFE_EVS_MAX_MESSAGE_LENGTH);
/* Ensure that the packet is always terminated by a null character */
EVS_Packet.Message[CFE_EVS_MAX_MESSAGE_LENGTH-1] = '\0';
/* send event out software bus */
Status = CFE_SB_SendMsg((CFE_SB_Msg_t *) &EVS_Packet);
}
}
return Status;
} /* End EVS_SendEventInternal */
void SIMCI_IngestPacket(void) {
int32 bytes_received;
uint8 buffer[SIMCI_MAX_INGEST];
bytes_received = recvfrom(SIMCI_AppData.IngestSocketID, buffer, SIMCI_MAX_INGEST, MSG_DONTWAIT, 0, 0);
if (bytes_received == ERROR) {
SIMCI_AppData.HkPacket.ingest_errors++;
CFE_EVS_SendEvent(SIMCI_INGEST_ERR_EID, CFE_EVS_ERROR, "SIMCI: Error receiving from ingest socket, errno=0x%08X", errno);
} else if (bytes_received <= SIMCI_MAX_INGEST) {
CFE_ES_PerfLogEntry(SIMCI_INGEST_RCV_PERF_ID);
SIMCI_AppData.HkPacket.ingest_packets++;
/* Send the prepared command out to SB */
CFE_SB_SendMsg((CFE_SB_MsgPtr_t)buffer);
CFE_ES_PerfLogExit(SIMCI_INGEST_RCV_PERF_ID);
} else {
SIMCI_AppData.HkPacket.ingest_errors++;
CFE_EVS_SendEvent(SIMCI_INGEST_ERR_EID, CFE_EVS_ERROR, "SIMCI: Dropped problematically lengthy ingest frame of %u bytes", bytes_received);
}
}
boolean EIM_NavDataUnpackAll(NavData_t* navdata, uint32* cks)
{
boolean success = TRUE;
NavData_Cks_t navdata_cks;
navdata_cks.cks = 0;
NavData_Option_t* navdata_option_ptr;
navdata_option_ptr = (NavData_Option_t*) &navdata->options[0];
EIM_AppData.nd_seq = navdata->sequence;
EIM_AppData.Hk.VisionDefined = navdata->vision_defined;
while( navdata_option_ptr != 0 )
{
// Check if we have a valid option
if( navdata_option_ptr->size == 0 )
{
/* TODO: Replace this with an error event. */
//PRINT("One option (%d) is not a valid option because its size is zero\n", navdata_option_ptr->tag);
navdata_option_ptr = 0;
success = FALSE;
}
else
{
switch( navdata_option_ptr->tag )
{
case ARDRONE2_NAVDATA_DEMO_TAG:
{
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_demo ,
sizeof(EIM_AppData.navdata_demo) );
EIM_AppData.Hk.navdata_demo_count++;
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &EIM_AppData.navdata_demo);
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_demo);
break;
}
case ARDRONE2_NAVDATA_TIME_TAG:
{
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_time,
sizeof(EIM_AppData.navdata_time) );
EIM_AppData.Hk.navdata_time_count++;
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &EIM_AppData.navdata_time);
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_time);
break;
}
case ARDRONE2_NAVDATA_RAW_MEASURES_TAG:
{
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_raw_measures ,
sizeof(EIM_AppData.navdata_raw_measures) );
EIM_AppData.Hk.navdata_raw_measures_count++;
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &EIM_AppData.navdata_raw_measures);
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_raw_measures);
EIM_AppData.ImuMsg.SampleCnt = 1;
EIM_AppData.ImuMsg.ImuSample[0].MsgAccData[0] = EIM_AppData.navdata_raw_measures.raw_accs[0];
EIM_AppData.ImuMsg.ImuSample[0].MsgAccData[1] = EIM_AppData.navdata_raw_measures.raw_accs[1];
EIM_AppData.ImuMsg.ImuSample[0].MsgAccData[2] = EIM_AppData.navdata_raw_measures.raw_accs[2];
EIM_AppData.ImuMsg.ImuSample[0].MsgRateData[0] = EIM_AppData.navdata_raw_measures.raw_gyros[0];
EIM_AppData.ImuMsg.ImuSample[0].MsgRateData[1] = EIM_AppData.navdata_raw_measures.raw_gyros[1];
EIM_AppData.ImuMsg.ImuSample[0].MsgRateData[2] = EIM_AppData.navdata_raw_measures.raw_gyros[2];
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &EIM_AppData.ImuMsg);
EIM_AppData.ImuMsg.ImuSample[0].TimeStamp = CFE_TIME_GetTime();
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.ImuMsg);
break;
}
case ARDRONE2_NAVDATA_PHYS_MEASURES_TAG:
{
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_phys_measures ,
sizeof(EIM_AppData.navdata_phys_measures) );
EIM_AppData.Hk.navdata_phys_measures_count++;
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &EIM_AppData.navdata_phys_measures);
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_phys_measures);
EIM_AppData.ImuMsg.SampleCnt = 1;
EIM_AppData.ImuMsg.ImuSample[0].MsgAccData[0] = EIM_AppData.navdata_phys_measures.phys_accs[0];
EIM_AppData.ImuMsg.ImuSample[0].MsgAccData[1] = EIM_AppData.navdata_phys_measures.phys_accs[1];
EIM_AppData.ImuMsg.ImuSample[0].MsgAccData[2] = EIM_AppData.navdata_phys_measures.phys_accs[2];
EIM_AppData.ImuMsg.ImuSample[0].MsgRateData[0] = EIM_AppData.navdata_phys_measures.phys_gyros[0];
EIM_AppData.ImuMsg.ImuSample[0].MsgRateData[1] = EIM_AppData.navdata_phys_measures.phys_gyros[1];
EIM_AppData.ImuMsg.ImuSample[0].MsgRateData[2] = EIM_AppData.navdata_phys_measures.phys_gyros[2];
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &EIM_AppData.ImuMsg);
EIM_AppData.ImuMsg.ImuSample[0].TimeStamp = CFE_TIME_GetTime();
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.ImuMsg);
break;
}
case ARDRONE2_NAVDATA_GYROS_OFFSETS_TAG:
{
EIM_AppData.Hk.navdata_gyro_offsets_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_gyro_offsets ,
sizeof(EIM_AppData.navdata_gyro_offsets) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_gyro_offsets);
break;
}
case ARDRONE2_NAVDATA_EULER_ANGLES_TAG:
{
EIM_AppData.Hk.navdata_euler_angles_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_euler_angles ,
sizeof(EIM_AppData.navdata_euler_angles) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_euler_angles);
break;
}
case ARDRONE2_NAVDATA_REFERENCES_TAG:
{
EIM_AppData.Hk.navdata_references_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_references ,
sizeof(EIM_AppData.navdata_references) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_references);
break;
}
case ARDRONE2_NAVDATA_TRIMS_TAG:
{
EIM_AppData.Hk.navdata_trims_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_trims,
sizeof(EIM_AppData.navdata_trims));
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_trims);
break;
}
case ARDRONE2_NAVDATA_RC_REFERENCES_TAG:
{
EIM_AppData.Hk.navdata_rc_references_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_rc_references ,
sizeof(EIM_AppData.navdata_rc_references) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_rc_references);
break;
}
case ARDRONE2_NAVDATA_PWM_TAG:
{
EIM_AppData.Hk.navdata_pwm_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_pwm ,
sizeof(EIM_AppData.navdata_pwm) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_pwm);
break;
}
case ARDRONE2_NAVDATA_ALTITUDE_TAG:
{
EIM_AppData.Hk.navdata_altitude_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_altitude ,
sizeof(EIM_AppData.navdata_altitude) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_altitude);
break;
}
case ARDRONE2_NAVDATA_VISION_RAW_TAG:
{
EIM_AppData.Hk.navdata_vision_raw_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_vision_raw ,
sizeof(EIM_AppData.navdata_vision_raw) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_vision_raw);
break;
}
case ARDRONE2_NAVDATA_VISION_OF_TAG:
{
EIM_AppData.Hk.navdata_vision_of_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_vision_of ,
sizeof(EIM_AppData.navdata_vision_of) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_vision_of);
break;
}
case ARDRONE2_NAVDATA_VISION_TAG:
{
EIM_AppData.Hk.navdata_vision_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_vision ,
sizeof(EIM_AppData.navdata_vision) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_vision);
break;
}
case ARDRONE2_NAVDATA_VISION_PERF_TAG:
{
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_vision_perf,
sizeof(EIM_AppData.navdata_vision_perf) );
EIM_AppData.Hk.navdata_vision_perf_count++;
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &EIM_AppData.navdata_vision_perf);
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_vision_perf);
break;
}
case ARDRONE2_NAVDATA_TRACKERS_SEND_TAG:
{
EIM_AppData.Hk.navdata_trackers_send_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_trackers_send ,
sizeof(EIM_AppData.navdata_trackers_send) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_trackers_send);
break;
}
case ARDRONE2_NAVDATA_VISION_DETECT_TAG:
{
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_vision_detect,
sizeof(EIM_AppData.navdata_vision_detect) );
EIM_AppData.Hk.navdata_vision_detect_count++;
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &EIM_AppData.navdata_vision_detect);
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_vision_detect);
break;
}
case ARDRONE2_NAVDATA_WATCHDOG_TAG:
{
EIM_AppData.Hk.navdata_watchdog_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_watchdog ,
sizeof(EIM_AppData.navdata_watchdog) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_watchdog);
break;
}
case ARDRONE2_NAVDATA_ADC_DATA_FRAME_TAG:
{
EIM_AppData.Hk.navdata_adc_data_frame_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_adc_data_frame ,
sizeof(EIM_AppData.navdata_adc_data_frame) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_adc_data_frame);
break;
}
case ARDRONE2_NAVDATA_VIDEO_STREAM_TAG:
{
EIM_AppData.Hk.navdata_video_stream_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_video_stream ,
sizeof(EIM_AppData.navdata_video_stream) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_video_stream);
break;
}
case ARDRONE2_NAVDATA_GAMES_TAG:
{
EIM_AppData.Hk.navdata_games_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_games ,
sizeof(EIM_AppData.navdata_games) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_games);
break;
}
case ARDRONE2_NAVDATA_PRESSURE_RAW_TAG:
{
EIM_AppData.Hk.navdata_pressure_raw_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_pressure_raw ,
sizeof(EIM_AppData.navdata_pressure_raw) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_pressure_raw);
break;
}
case ARDRONE2_NAVDATA_MAGNETO_TAG:
{
EIM_AppData.Hk.navdata_magneto_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_magneto ,
sizeof(EIM_AppData.navdata_magneto) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_magneto);
break;
}
case ARDRONE2_NAVDATA_WIND_TAG:
{
EIM_AppData.Hk.navdata_wind_speed_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_wind_speed ,
sizeof(EIM_AppData.navdata_wind_speed) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_wind_speed);
break;
}
case ARDRONE2_NAVDATA_KALMAN_PRESSURE_TAG:
{
EIM_AppData.Hk.navdata_kalman_pressure_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_kalman_pressure ,
sizeof(EIM_AppData.navdata_kalman_pressure) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_kalman_pressure);
break;
}
case ARDRONE2_NAVDATA_HDVIDEO_STREAM_TAG:
{
EIM_AppData.Hk.navdata_hdvideo_stream_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_hdvideo_stream ,
sizeof(EIM_AppData.navdata_hdvideo_stream) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_hdvideo_stream);
break;
}
case ARDRONE2_NAVDATA_WIFI_TAG:
{
EIM_AppData.Hk.navdata_wifi_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_wifi ,
sizeof(EIM_AppData.navdata_wifi) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_wifi);
break;
}
case ARDRONE2_NAVDATA_ZIMMU_3000_TAG:
{
EIM_AppData.Hk.navdata_zimmu_3000_count++;
navdata_option_ptr = EIM_NavdataUnpackOption( navdata_option_ptr,
(CFE_SB_Msg_t *) &EIM_AppData.navdata_zimmu_3000 ,
sizeof(EIM_AppData.navdata_zimmu_3000) );
CFE_SB_SendMsg((CFE_SB_Msg_t*)&EIM_AppData.navdata_zimmu_3000);
break;
}
case ARDRONE2_NAVDATA_CKS_TAG:
{
//navdata_option_ptr = ARDrone2_NavdataUnpackOption( navdata_option_ptr,
//(NavDataHeader_t *) &navdata_unpacked->demo ,
//sizeof(navdata_unpacked->demo) );
*cks = navdata_cks.cks;
navdata_option_ptr = 0; // End of structure
break;
}
default:
{
// TODO: Replace the following with an error event.
//PRINT("Tag %d is an unknown navdata option tag\n", (int) navdata_option_ptr->tag);
navdata_option_ptr = (NavData_Option_t*)(((uint32)navdata_option_ptr) + navdata_option_ptr->size);
break;
}
}
}
}
return success;
}
int32 SCH_HousekeepingCmd(CFE_SB_MsgPtr_t MessagePtr)
{
int32 TableResult = SCH_SUCCESS;
if(SCH_VerifyCmdLength(MessagePtr, sizeof(SCH_NoArgsCmd_t)) == SCH_SUCCESS)
{
/*
** Update contents of Housekeeping Packet
*/
SCH_AppData.HkPacket.CmdCounter = SCH_AppData.CmdCounter;
SCH_AppData.HkPacket.ErrCounter = SCH_AppData.ErrCounter;
SCH_AppData.HkPacket.ScheduleActivitySuccessCount = SCH_AppData.ScheduleActivitySuccessCount;
SCH_AppData.HkPacket.ScheduleActivityFailureCount = SCH_AppData.ScheduleActivityFailureCount;
SCH_AppData.HkPacket.SlotsProcessedCount = SCH_AppData.SlotsProcessedCount;
SCH_AppData.HkPacket.SkippedSlotsCount = SCH_AppData.SkippedSlotsCount;
SCH_AppData.HkPacket.MultipleSlotsCount = SCH_AppData.MultipleSlotsCount;
SCH_AppData.HkPacket.SameSlotCount = SCH_AppData.SameSlotCount;
SCH_AppData.HkPacket.BadTableDataCount = SCH_AppData.BadTableDataCount;
SCH_AppData.HkPacket.TableVerifySuccessCount = SCH_AppData.TableVerifySuccessCount;
SCH_AppData.HkPacket.TableVerifyFailureCount = SCH_AppData.TableVerifyFailureCount;
SCH_AppData.HkPacket.TablePassCount = SCH_AppData.TablePassCount;
SCH_AppData.HkPacket.ValidMajorFrameCount = SCH_AppData.ValidMajorFrameCount;
SCH_AppData.HkPacket.MissedMajorFrameCount = SCH_AppData.MissedMajorFrameCount;
SCH_AppData.HkPacket.UnexpectedMajorFrameCount = SCH_AppData.UnexpectedMajorFrameCount;
SCH_AppData.HkPacket.MinorFramesSinceTone = SCH_AppData.MinorFramesSinceTone;
SCH_AppData.HkPacket.NextSlotNumber = SCH_AppData.NextSlotNumber;
SCH_AppData.HkPacket.LastSyncMETSlot = SCH_AppData.LastSyncMETSlot;
SCH_AppData.HkPacket.IgnoreMajorFrame = SCH_AppData.IgnoreMajorFrame;
SCH_AppData.HkPacket.UnexpectedMajorFrame = SCH_AppData.UnexpectedMajorFrame;
SCH_AppData.HkPacket.SyncToMET = SCH_AppData.SyncToMET;
SCH_AppData.HkPacket.MajorFrameSource = SCH_AppData.MajorFrameSource;
/*
** Timestamps and send housekeeping packet
*/
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &SCH_AppData.HkPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &SCH_AppData.HkPacket);
/*
** Reset "high rate" event filters
*/
CFE_EVS_ResetAllFilters();
}
/*
** Note:
**
** The following table functions will give the cFE Table Manager
** a chance to update the tables used by this application. If
** there is an error (very unlikely) the return value will cause
** us to fall out of the main process loop and terminate the SCH
** task. It may sound extreme but there is nothing for the
** Scheduler to do if it cannot access both the message
** and schedule tables.
*/
CFE_TBL_ReleaseAddress(SCH_AppData.ScheduleTableHandle);
CFE_TBL_ReleaseAddress(SCH_AppData.MessageTableHandle);
TableResult = SCH_AcquirePointers();
return(TableResult);
} /* End of SCH_HousekeepingCmd() */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
int32 CFE_ES_ShellOutputCommand(char * CmdString, char *Filename)
{
int32 Result;
int32 ReturnCode = CFE_SUCCESS;
int32 fd;
int32 FileSize;
int32 CurrFilePtr;
uint32 i;
/* the extra 1 added for the \0 char */
char CheckCmd [CFE_ES_CHECKSIZE + 1];
char Cmd [CFE_ES_MAX_SHELL_CMD];
char OutputFilename [OS_MAX_PATH_LEN];
/* Use default filename if not provided */
if (Filename[0] == '\0')
{
strncpy(OutputFilename, CFE_ES_DEFAULT_SHELL_FILENAME, OS_MAX_PATH_LEN);
}
else
{
strncpy(OutputFilename, Filename, OS_MAX_PATH_LEN);
}
/* Make sure string is null terminated */
OutputFilename[OS_MAX_PATH_LEN - 1] = '\0';
/* Remove previous version of output file */
OS_remove(OutputFilename);
fd = OS_creat(OutputFilename, OS_READ_WRITE);
if (fd < OS_FS_SUCCESS)
{
Result = OS_FS_ERROR;
}
else
{
strncpy(CheckCmd,CmdString,CFE_ES_CHECKSIZE);
CheckCmd[CFE_ES_CHECKSIZE] = '\0';
strncpy(Cmd,CmdString, CFE_ES_MAX_SHELL_CMD);
/* We need to check if this command is directed at ES, or at the
operating system */
if (strncmp(CheckCmd,"ES_",CFE_ES_CHECKSIZE) == 0)
{
/* This list can be expanded to include other ES functionality */
if ( strncmp(Cmd,CFE_ES_LIST_APPS_CMD,strlen(CFE_ES_LIST_APPS_CMD) )== 0)
{
Result = CFE_ES_ListApplications(fd);
}
else if ( strncmp(Cmd,CFE_ES_LIST_TASKS_CMD,strlen(CFE_ES_LIST_TASKS_CMD) )== 0)
{
Result = CFE_ES_ListTasks(fd);
}
else if ( strncmp(Cmd,CFE_ES_LIST_RESOURCES_CMD,strlen(CFE_ES_LIST_RESOURCES_CMD) )== 0)
{
Result = CFE_ES_ListResources(fd);
}
/* default if there is not an ES command that matches */
else
{
Result = CFE_ES_ERR_SHELL_CMD;
CFE_ES_WriteToSysLog("There is no ES Shell command that matches %s \n",Cmd);
}
}
/* if the command is not directed at ES, pass it through to the
* underlying OS */
else
{
Result = OS_ShellOutputToFile(Cmd,fd);
}
/* seek to the end of the file to get it's size */
FileSize = OS_lseek(fd,0,OS_SEEK_END);
if (FileSize == OS_FS_ERROR)
{
OS_close(fd);
CFE_ES_WriteToSysLog("OS_lseek call failed from CFE_ES_ShellOutputCmd 1\n");
Result = OS_FS_ERROR;
}
/* We want to add 3 characters at the end of the telemetry,'\n','$','\0'.
* To do this we need to make sure there are at least 3 empty characters at
* the end of the last CFE_ES_MAX_SHELL_PKT so we don't over write any data. If
* the current file has only 0,1, or 2 free spaces at the end, we want to
* make the file longer to start a new tlm packet of size CFE_ES_MAX_SHELL_PKT.
* This way we will get a 'blank' packet with the correct 3 characters at the end.
*/
else
{
/* if we are within 2 bytes of the end of the packet*/
if ( FileSize % CFE_ES_MAX_SHELL_PKT > (CFE_ES_MAX_SHELL_PKT - 3))
{
/* add enough bytes to start a new packet */
for (i = 0; i < CFE_ES_MAX_SHELL_PKT - (FileSize % CFE_ES_MAX_SHELL_PKT) + 1 ; i++)
{
OS_write(fd," ",1);
}
}
else
{
/* we are exactly at the end */
if( FileSize % CFE_ES_MAX_SHELL_PKT == 0)
{
OS_write(fd," ",1);
}
}
/* seek to the end of the file again to get it's new size */
FileSize = OS_lseek(fd,0,OS_SEEK_END);
if (FileSize == OS_FS_ERROR)
{
OS_close(fd);
CFE_ES_WriteToSysLog("OS_lseek call failed from CFE_ES_ShellOutputCmd 2\n");
Result = OS_FS_ERROR;
}
else
{
/* set the file back to the beginning */
OS_lseek(fd,0,OS_SEEK_SET);
/* start processing the chunks. We want to have one packet left so we are sure this for loop
* won't run over */
for (CurrFilePtr=0; CurrFilePtr < (FileSize - CFE_ES_MAX_SHELL_PKT); CurrFilePtr += CFE_ES_MAX_SHELL_PKT)
{
OS_read(fd, CFE_ES_TaskData.ShellPacket.ShellOutput, CFE_ES_MAX_SHELL_PKT);
/* Send the packet */
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &CFE_ES_TaskData.ShellPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &CFE_ES_TaskData.ShellPacket);
/* delay to not flood the pipe on large messages */
OS_TaskDelay(200);
}
/* finish off the last portion of the file */
/* over write the last packet with spaces, then it will get filled
* in with the correct info below. This assures that the last non full
* part of the packet will be spaces */
for (i =0; i < CFE_ES_MAX_SHELL_PKT; i++)
{
CFE_ES_TaskData.ShellPacket.ShellOutput[i] = ' ';
}
OS_read(fd, CFE_ES_TaskData.ShellPacket.ShellOutput, ( FileSize - CurrFilePtr));
/* From our check above, we are assured that there are at least 3 free
* characters to write our data into at the end of this last packet
*
* The \n assures we are on a new line, the $ gives us our prompt, and the
* \0 assures we are null terminalted.
*/
CFE_ES_TaskData.ShellPacket.ShellOutput[ CFE_ES_MAX_SHELL_PKT - 3] = '\n';
CFE_ES_TaskData.ShellPacket.ShellOutput[ CFE_ES_MAX_SHELL_PKT - 2] = '$';
CFE_ES_TaskData.ShellPacket.ShellOutput[ CFE_ES_MAX_SHELL_PKT - 1] = '\0';
/* Send the last packet */
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &CFE_ES_TaskData.ShellPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &CFE_ES_TaskData.ShellPacket);
/* Close the file descriptor */
OS_close(fd);
} /* if FilseSize == OS_FS_ERROR */
} /* if FileSeize == OS_FS_ERROR */
}/* if fd < OS_FS_SUCCESS */
if (Result != OS_SUCCESS && Result != CFE_SUCCESS )
{
ReturnCode = CFE_ES_ERR_SHELL_CMD;
CFE_ES_WriteToSysLog("OS_ShellOutputToFile call failed from CFE_ES_ShellOutputCommand\n");
}
else
{
ReturnCode = CFE_SUCCESS;
}
return ReturnCode;
}
/*
** Send a message via the software bus
*/
void UT_SendMsg(CFE_SB_MsgPtr_t msg_ptr, CFE_SB_MsgId_t id, uint16 code)
{
CFE_SB_SetMsgId(msg_ptr, id);
CFE_SB_SetCmdCode(msg_ptr, code);
CFE_SB_SendMsg(msg_ptr);
}
void THRSIM_ReportHousekeeping(void) {
CFE_SB_TimeStampMsg((CFE_SB_MsgPtr_t) &THRSIM_AppData.HkPacket);
CFE_SB_SendMsg((CFE_SB_MsgPtr_t) &THRSIM_AppData.HkPacket);
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void TO_output_status(void)
{
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &to_hk_status);
CFE_SB_SendMsg((CFE_SB_Msg_t *)&to_hk_status);
} /* End of TO_output_status() */
void CI_ReadUpLink(void)
{
socklen_t addr_len;
int i;
int status;
addr_len = sizeof(CI_SocketAddress);
bzero((char *) &CI_SocketAddress, sizeof(CI_SocketAddress));
for (i = 0; i <= 10; i++)
{
status = recvfrom(CI_SocketID, (char *)&CI_IngestBuffer[0], sizeof(CI_IngestBuffer), MSG_DONTWAIT,
(struct sockaddr *) &CI_SocketAddress, &addr_len);
if ( (status < 0) && (errno == EWOULDBLOCK) )
break; /* no (more) messages */
else
{
if (status <= CI_MAX_INGEST)
{
CCSDS_PriHdr_t *PrimaryHeader = NULL;
CFE_ES_PerfLogEntry(CI_SOCKET_RCV_PERF_ID);
CI_HkTelemetryPkt.IngestPackets++;
#ifdef SOFTWARE_LITTLE_BIT_ORDER
PrimaryHeader = (CCSDS_PriHdr_t*)CI_IngestPointer;
PrimaryHeader->StreamId = ntohs(PrimaryHeader->StreamId);
PrimaryHeader->Sequence = ntohs(PrimaryHeader->Sequence);
PrimaryHeader->Length = ntohs(PrimaryHeader->Length);
if (CCSDS_SID_TYPE(PrimaryHeader->StreamId) == CCSDS_CMD) {
CCSDS_CmdPkt_t *CmdPkt = (CCSDS_CmdPkt_t*)PrimaryHeader;
CmdPkt->SecHdr.Command = ntohs(CmdPkt->SecHdr.Command);
} else {
CCSDS_TlmPkt_t *TlmPkt = (CCSDS_TlmPkt_t*)PrimaryHeader;
uint32 *SecondsPtr = ((uint32 *)&TlmPkt->SecHdr.Time[0]);
uint16 *SubsecondsPtr = ((uint16 *)&TlmPkt->SecHdr.Time[4]);
*SecondsPtr = ntohs(*SecondsPtr);
*SubsecondsPtr = ntohs(*SubsecondsPtr);
}
#endif
#ifdef DO_DEBUG
{
/*LOGAN - print out hex command message exactly as it was sent */
int i;
unsigned char *mpp = (unsigned char*) &CI_IngestPointer[0];
for( i=0; i< CFE_SB_GetTotalMsgLength(CI_IngestPointer); i++ ) { printf("%02x ", mpp[i] ); }
printf("\n");
}
#endif
CFE_SB_SendMsg(CI_IngestPointer);
CFE_ES_PerfLogExit(CI_SOCKET_RCV_PERF_ID);
}
else
{
CI_HkTelemetryPkt.IngestErrors++;
CFE_EVS_SendEvent(CI_INGEST_ERR_EID,CFE_EVS_ERROR, "CI: L%d, cmd %0x %0x dropped, too long\n", __LINE__, *(long *)CI_IngestBuffer, *(long *)(CI_IngestBuffer+4) );
}
}
}
return;
} /* End of CI_ReadUpLink() */
