/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ void DS_FileCreateSequence(char *Buffer, uint32 Type, uint32 Count) { CFE_TIME_SysTime_t TimeToPrint; uint32 SequenceCount; uint32 NumericDigit; int32 BufferIndex; /* ** Build the sequence portion of the filename (time or count)... */ if (Type == DS_BY_COUNT) { /* ** Get copy of sequence count that can be modified... */ SequenceCount = Count; /* ** Extract each digit (least significant digit first)... */ for (BufferIndex = DS_SEQUENCE_DIGITS - 1; BufferIndex >= 0; BufferIndex--) { /* ** Extract this digit and prepare for next digit... */ NumericDigit = SequenceCount % 10; SequenceCount = SequenceCount / 10; /* ** Store this digit as ASCII in sequence string buffer... */ Buffer[BufferIndex] = '0' + NumericDigit; } /* ** Add string terminator... */ Buffer[DS_SEQUENCE_DIGITS] = DS_STRING_TERMINATOR; } else if (Type == DS_BY_TIME) { #if (DS_FILE_HEADER_TYPE == DS_FILE_HEADER_GPM) /* ** Filename is based on seconds from current packet timestamp... */ TimeToPrint.Seconds = DS_AppData.CurrentPktTime; TimeToPrint.Subseconds = 0; #else /* ** Filename is based on seconds from current time... */ TimeToPrint = CFE_TIME_GetTime(); #endif /* ** Convert time value to cFE format text string... */ CFE_TIME_Print(Buffer, TimeToPrint); /* ** cFE time string has format: "YYYY-DDD-HH:MM:SS.sssss"... */ #define CFE_YYYY_INDEX 0 #define CFE_DDD_INDEX 5 #define CFE_HH_INDEX 9 #define CFE_MM_INDEX 12 #define CFE_SS_INDEX 15 #define CFE_ssss_INDEX 18 /* ** DS time string has format: "YYYYDDDHHMMSS"... */ #define DS_YYYY_INDEX 0 #define DS_DDD_INDEX 4 #define DS_HH_INDEX 7 #define DS_MM_INDEX 9 #define DS_SS_INDEX 11 #define DS_TERM_INDEX 13 /* ** Convert cFE time string to DS time string by moving ** the cFE chars to the left to remove extra stuff... */ /* ** Step 1: Leave "year" (YYYY) alone - it is already OK... */ /* ** Step 2: Move "day of year" (DDD) next to (YYYY)... */ Buffer[DS_DDD_INDEX + 0] = Buffer[CFE_DDD_INDEX + 0]; Buffer[DS_DDD_INDEX + 1] = Buffer[CFE_DDD_INDEX + 1]; Buffer[DS_DDD_INDEX + 2] = Buffer[CFE_DDD_INDEX + 2]; /* ** Step 3: Move "hour of day" (HH) next to (DDD)... */ Buffer[DS_HH_INDEX + 0] = Buffer[CFE_HH_INDEX + 0]; Buffer[DS_HH_INDEX + 1] = Buffer[CFE_HH_INDEX + 1]; /* ** Step 4: Move "minutes" (MM) next to (HH)... */ Buffer[DS_MM_INDEX + 0] = Buffer[CFE_MM_INDEX + 0]; Buffer[DS_MM_INDEX + 1] = Buffer[CFE_MM_INDEX + 1]; /* ** Step 5: Move "seconds" (SS) next to (MM)... */ Buffer[DS_SS_INDEX + 0] = Buffer[CFE_SS_INDEX + 0]; Buffer[DS_SS_INDEX + 1] = Buffer[CFE_SS_INDEX + 1]; /* ** Step 6: Skip "subsecs" (ssss) - not in DS format... */ /* ** Step 7: Add string terminator... */ Buffer[DS_TERM_INDEX] = DS_STRING_TERMINATOR; } else { /* ** Bad filename type, init buffer as empty... */ Buffer[0] = DS_STRING_TERMINATOR; } return; } /* End of DS_FileCreateSequence() */
void DS_FileUpdateHeader(int32 FileIndex) { #if (DS_FILE_HEADER_TYPE == DS_FILE_HEADER_CFE) /* ** Update CFE specific header fields... */ DS_AppFileStatus_t *FileStatus = &DS_AppData.FileStatus[FileIndex]; CFE_TIME_SysTime_t CurrentTime = CFE_TIME_GetTime(); int32 Result; Result = OS_lseek(FileStatus->FileHandle, sizeof(CFE_FS_Header_t), SEEK_SET); if (Result == sizeof(CFE_FS_Header_t)) { /* update file close time */ Result = OS_write(FileStatus->FileHandle, &CurrentTime, sizeof(CFE_TIME_SysTime_t)); if (Result == sizeof(CFE_TIME_SysTime_t)) { DS_AppData.FileUpdateCounter++; } else { DS_AppData.FileUpdateErrCounter++; } } else { DS_AppData.FileUpdateErrCounter++; } #elif (DS_FILE_HEADER_TYPE == DS_FILE_HEADER_GPM) /* ** Update GPM specific header fields... */ DS_AppFileStatus_t *FileStatus = &DS_AppData.FileStatus[FileIndex]; char Buf8[8]; int32 Result; Result = OS_lseek(FileStatus->FileHandle, 16, SEEK_SET); if (Result == 16) { /* update last pkt time in file header */ Result = OS_write(FileStatus->FileHandle, &DS_AppData.LastPktTime[FileIndex], sizeof(uint32)); if (Result == sizeof(uint32)) { /* update file size in file header */ CFE_PSP_MemSet(Buf8, ' ', sizeof(Buf8)); DS_FileConvertGPM(Buf8, FileStatus->FileSize); Result = OS_write(FileStatus->FileHandle, Buf8, sizeof(Buf8)); if (Result == sizeof(Buf8)) { DS_AppData.FileUpdateCounter++; } else { DS_AppData.FileUpdateErrCounter++; } } else { DS_AppData.FileUpdateErrCounter++; } } else { DS_AppData.FileUpdateErrCounter++; } #endif return; } /* End of DS_FileUpdateHeader() */
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; }