restructure repository
This commit is contained in:
614
src/core/timemanager/CCSDSTime.cpp
Normal file
614
src/core/timemanager/CCSDSTime.cpp
Normal file
@ -0,0 +1,614 @@
|
||||
#include "CCSDSTime.h"
|
||||
#include <FSFWConfig.h>
|
||||
#include <cstdio>
|
||||
#include <cinttypes>
|
||||
#include <cmath>
|
||||
|
||||
|
||||
CCSDSTime::CCSDSTime() {
|
||||
}
|
||||
|
||||
CCSDSTime::~CCSDSTime() {
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertToCcsds(Ccs_seconds* to,
|
||||
const Clock::TimeOfDay_t* from) {
|
||||
ReturnValue_t result = checkTimeOfDay(from);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
to->pField = (CCS << 4);
|
||||
|
||||
to->yearMSB = (from->year >> 8);
|
||||
to->yearLSB = from->year & 0xff;
|
||||
to->month = from->month;
|
||||
to->day = from->day;
|
||||
to->hour = from->hour;
|
||||
to->minute = from->minute;
|
||||
to->second = from->second;
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertToCcsds(Ccs_mseconds* to,
|
||||
const Clock::TimeOfDay_t* from) {
|
||||
ReturnValue_t result = checkTimeOfDay(from);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
to->pField = (CCS << 4) + 2;
|
||||
|
||||
to->yearMSB = (from->year >> 8);
|
||||
to->yearLSB = from->year & 0xff;
|
||||
to->month = from->month;
|
||||
to->day = from->day;
|
||||
to->hour = from->hour;
|
||||
to->minute = from->minute;
|
||||
to->second = from->second;
|
||||
to->secondEminus2 = from->usecond / 10000;
|
||||
to->secondEminus4 = (from->usecond % 10000) / 100;
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCcsds(Clock::TimeOfDay_t* to,
|
||||
const uint8_t* from, size_t length) {
|
||||
ReturnValue_t result;
|
||||
if (length > 0xFF) {
|
||||
return LENGTH_MISMATCH;
|
||||
}
|
||||
result = convertFromASCII(to, from, length); //Try to parse it as ASCII
|
||||
if (result == RETURN_OK) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
//Seems to be no ascii, try the other formats
|
||||
uint8_t codeIdentification = (*from >> 4);
|
||||
switch (codeIdentification) {
|
||||
case CUC_LEVEL1: //CUC_LEVEL2 can not be converted to TimeOfDay (ToD is Level 1) <- Well, if we know the epoch, we can... <- see bug 1133
|
||||
return convertFromCUC(to, from, length);
|
||||
case CDS:
|
||||
return convertFromCDS(to, from, length);
|
||||
case CCS: {
|
||||
size_t temp = 0;
|
||||
return convertFromCCS(to, from, &temp, length);
|
||||
}
|
||||
|
||||
default:
|
||||
return UNSUPPORTED_TIME_FORMAT;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCUC(Clock::TimeOfDay_t* to,
|
||||
const uint8_t* from, uint8_t length) {
|
||||
return UNSUPPORTED_TIME_FORMAT;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCDS(Clock::TimeOfDay_t* to,
|
||||
const uint8_t* from, uint8_t length) {
|
||||
timeval time;
|
||||
ReturnValue_t result = convertFromCDS(&time, from, NULL, length);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
return convertTimevalToTimeOfDay(to, &time);
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCCS(Clock::TimeOfDay_t* to,
|
||||
const uint8_t* from, size_t* foundLength, size_t maxLength) {
|
||||
uint8_t subsecondsLength = *from & 0b111;
|
||||
uint32_t totalLength = subsecondsLength + 8;
|
||||
if (maxLength < totalLength) {
|
||||
return LENGTH_MISMATCH;
|
||||
}
|
||||
|
||||
*foundLength = totalLength;
|
||||
|
||||
ReturnValue_t result = checkCcs(from, maxLength);
|
||||
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
Ccs_mseconds *temp = (Ccs_mseconds *) from;
|
||||
|
||||
to->year = (temp->yearMSB << 8) + temp->yearLSB;
|
||||
to->hour = temp->hour;
|
||||
to->minute = temp->minute;
|
||||
to->second = temp->second;
|
||||
|
||||
if (temp->pField & (1 << 3)) { //day of year variation
|
||||
uint16_t tempDay = (temp->month << 8) + temp->day;
|
||||
result = convertDaysOfYear(tempDay, to->year,
|
||||
&(temp->month), &(temp->day));
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
to->month = temp->month;
|
||||
to->day = temp->day;
|
||||
|
||||
to->usecond = 0;
|
||||
if (subsecondsLength > 0) {
|
||||
*foundLength += 1;
|
||||
if (temp->secondEminus2 >= 100) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
to->usecond = temp->secondEminus2 * 10000;
|
||||
}
|
||||
|
||||
if (subsecondsLength > 1) {
|
||||
*foundLength += 1;
|
||||
if (temp->secondEminus4 >= 100) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
to->usecond += temp->secondEminus4 * 100;
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to,
|
||||
const uint8_t* from, uint8_t length) {
|
||||
if (length < 19) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
// Newlib nano can't parse uint8, see SCNu8 documentation and https://sourceware.org/newlib/README
|
||||
// Suggestion: use uint16 all the time. This should work on all systems.
|
||||
#if FSFW_NO_C99_IO == 1
|
||||
uint16_t year;
|
||||
uint16_t month;
|
||||
uint16_t day;
|
||||
uint16_t hour;
|
||||
uint16_t minute;
|
||||
float second;
|
||||
int count = sscanf((char *) from, "%4" SCNu16 "-%2" SCNu16 "-%2" SCNu16 "T%"
|
||||
"2" SCNu16 ":%2" SCNu16 ":%fZ", &year, &month, &day, &hour,
|
||||
&minute, &second);
|
||||
if (count == 6) {
|
||||
to->year = year;
|
||||
to->month = month;
|
||||
to->day = day;
|
||||
to->hour = hour;
|
||||
to->minute = minute;
|
||||
to->second = second;
|
||||
to->usecond = (second - floor(second)) * 1000000;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
// try Code B (yyyy-ddd)
|
||||
count = sscanf((char *) from, "%4" SCNu16 "-%3" SCNu16 "T%2" SCNu16 ":%"
|
||||
"2" SCNu16 ":%fZ", &year, &day, &hour, &minute, &second);
|
||||
if (count == 5) {
|
||||
uint8_t tempDay;
|
||||
ReturnValue_t result = CCSDSTime::convertDaysOfYear(day, year,
|
||||
reinterpret_cast<uint8_t *>(&month),
|
||||
reinterpret_cast<uint8_t *>(&tempDay));
|
||||
if (result != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
to->year = year;
|
||||
to->month = month;
|
||||
to->day = tempDay;
|
||||
to->hour = hour;
|
||||
to->minute = minute;
|
||||
to->second = second;
|
||||
to->usecond = (second - floor(second)) * 1000000;
|
||||
return RETURN_OK;
|
||||
}
|
||||
// Warning: Compiler/Linker fails ambiguously if library does not implement
|
||||
// C99 I/O
|
||||
#else
|
||||
uint16_t year;
|
||||
uint8_t month;
|
||||
uint16_t day;
|
||||
uint8_t hour;
|
||||
uint8_t minute;
|
||||
float second;
|
||||
//try Code A (yyyy-mm-dd)
|
||||
int count = sscanf((char *) from, "%4" SCNu16 "-%2" SCNu8 "-%2" SCNu16
|
||||
"T%2" SCNu8 ":%2" SCNu8 ":%fZ", &year, &month, &day,
|
||||
&hour, &minute, &second);
|
||||
if (count == 6) {
|
||||
to->year = year;
|
||||
to->month = month;
|
||||
to->day = day;
|
||||
to->hour = hour;
|
||||
to->minute = minute;
|
||||
to->second = second;
|
||||
to->usecond = (second - floor(second)) * 1000000;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
//try Code B (yyyy-ddd)
|
||||
count = sscanf((char *) from, "%4" SCNu16 "-%3" SCNu16 "T%2" SCNu8
|
||||
":%2" SCNu8 ":%fZ", &year, &day, &hour, &minute, &second);
|
||||
if (count == 5) {
|
||||
uint8_t tempDay;
|
||||
ReturnValue_t result = CCSDSTime::convertDaysOfYear(day, year, &month,
|
||||
&tempDay);
|
||||
if (result != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
to->year = year;
|
||||
to->month = month;
|
||||
to->day = tempDay;
|
||||
to->hour = hour;
|
||||
to->minute = minute;
|
||||
to->second = second;
|
||||
to->usecond = (second - floor(second)) * 1000000;
|
||||
return RETURN_OK;
|
||||
}
|
||||
#endif
|
||||
|
||||
return UNSUPPORTED_TIME_FORMAT;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::checkCcs(const uint8_t* time, uint8_t length) {
|
||||
Ccs_mseconds *time_struct = (Ccs_mseconds *) time;
|
||||
|
||||
uint8_t additionalBytes = time_struct->pField & 0b111;
|
||||
if ((additionalBytes == 0b111) || (length < (additionalBytes + 8))) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
|
||||
if (time_struct->pField & (1 << 3)) { //day of year variation
|
||||
uint16_t day = (time_struct->month << 8) + time_struct->day;
|
||||
if (day > 366) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
} else {
|
||||
if (time_struct->month > 12) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
if (time_struct->day > 31) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
}
|
||||
if (time_struct->hour > 23) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
if (time_struct->minute > 59) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
if (time_struct->second > 59) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
|
||||
uint8_t *additionalByte = &time_struct->secondEminus2;
|
||||
|
||||
for (; additionalBytes != 0; additionalBytes--) {
|
||||
if (*additionalByte++ > 99) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertDaysOfYear(uint16_t dayofYear, uint16_t year,
|
||||
uint8_t* month, uint8_t* day) {
|
||||
if (isLeapYear(year)) {
|
||||
if (dayofYear > 366) {
|
||||
return INVALID_DAY_OF_YEAR;
|
||||
}
|
||||
} else {
|
||||
if (dayofYear > 365) {
|
||||
return INVALID_DAY_OF_YEAR;
|
||||
}
|
||||
}
|
||||
*month = 1;
|
||||
if (dayofYear <= 31) {
|
||||
*day = dayofYear;
|
||||
return RETURN_OK;
|
||||
}
|
||||
*month += 1;
|
||||
dayofYear -= 31;
|
||||
if (isLeapYear(year)) {
|
||||
if (dayofYear <= 29) {
|
||||
*day = dayofYear;
|
||||
return RETURN_OK;
|
||||
}
|
||||
*month += 1;
|
||||
dayofYear -= 29;
|
||||
} else {
|
||||
if (dayofYear <= 28) {
|
||||
*day = dayofYear;
|
||||
return RETURN_OK;
|
||||
}
|
||||
*month += 1;
|
||||
dayofYear -= 28;
|
||||
}
|
||||
while (*month <= 12) {
|
||||
if (dayofYear <= 31) {
|
||||
*day = dayofYear;
|
||||
return RETURN_OK;
|
||||
}
|
||||
*month += 1;
|
||||
dayofYear -= 31;
|
||||
|
||||
if (*month == 8) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (dayofYear <= 30) {
|
||||
*day = dayofYear;
|
||||
return RETURN_OK;
|
||||
}
|
||||
*month += 1;
|
||||
dayofYear -= 30;
|
||||
}
|
||||
return INVALID_DAY_OF_YEAR;
|
||||
}
|
||||
|
||||
bool CCSDSTime::isLeapYear(uint32_t year) {
|
||||
if ((year % 400) == 0) {
|
||||
return true;
|
||||
}
|
||||
if ((year % 100) == 0) {
|
||||
return false;
|
||||
}
|
||||
if ((year % 4) == 0) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertToCcsds(CDS_short* to, const timeval* from) {
|
||||
to->pField = (CDS << 4) + 0;
|
||||
uint32_t days = ((from->tv_sec) / SECONDS_PER_DAY)
|
||||
+ DAYS_CCSDS_TO_UNIX_EPOCH;
|
||||
if (days > (1 << 16)) {
|
||||
//Date is beyond year 2137
|
||||
return TIME_DOES_NOT_FIT_FORMAT;
|
||||
}
|
||||
to->dayMSB = (days & 0xFF00) >> 8;
|
||||
to->dayLSB = (days & 0xFF);
|
||||
uint32_t msDay = ((from->tv_sec % SECONDS_PER_DAY) * 1000)
|
||||
+ (from->tv_usec / 1000);
|
||||
to->msDay_hh = (msDay & 0xFF000000) >> 24;
|
||||
to->msDay_h = (msDay & 0xFF0000) >> 16;
|
||||
to->msDay_l = (msDay & 0xFF00) >> 8;
|
||||
to->msDay_ll = (msDay & 0xFF);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertToCcsds(OBT_FLP* to, const timeval* from) {
|
||||
to->pFiled = (AGENCY_DEFINED << 4) + 5;
|
||||
to->seconds_hh = (from->tv_sec >> 24) & 0xff;
|
||||
to->seconds_h = (from->tv_sec >> 16) & 0xff;
|
||||
to->seconds_l = (from->tv_sec >> 8) & 0xff;
|
||||
to->seconds_ll = (from->tv_sec >> 0) & 0xff;
|
||||
|
||||
//convert the µs to 2E-16 seconds
|
||||
uint64_t temp = from->tv_usec;
|
||||
temp = temp << 16;
|
||||
temp = temp / 1E6;
|
||||
|
||||
to->subsecondsMSB = (temp >> 8) & 0xff;
|
||||
to->subsecondsLSB = temp & 0xff;
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCcsds(timeval* to, const uint8_t* from,
|
||||
size_t* foundLength, size_t maxLength) {
|
||||
if(maxLength >= 19) {
|
||||
Clock::TimeOfDay_t timeOfDay;
|
||||
/* Try to parse it as ASCII */
|
||||
ReturnValue_t result = convertFromASCII(&timeOfDay, from, maxLength);
|
||||
if (result == RETURN_OK) {
|
||||
return Clock::convertTimeOfDayToTimeval(&timeOfDay, to);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t codeIdentification = (*from >> 4);
|
||||
switch (codeIdentification) {
|
||||
/* Unsupported, as Leap second correction would have to be applied */
|
||||
case CUC_LEVEL1:
|
||||
return UNSUPPORTED_TIME_FORMAT;
|
||||
case CDS:
|
||||
return convertFromCDS(to, from, foundLength, maxLength);
|
||||
case CCS:
|
||||
return convertFromCCS(to, from, foundLength, maxLength);
|
||||
default:
|
||||
return UNSUPPORTED_TIME_FORMAT;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCUC(timeval* to, const uint8_t* from,
|
||||
size_t* foundLength, size_t maxLength) {
|
||||
if (maxLength < 1) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
uint8_t pField = *from;
|
||||
from++;
|
||||
ReturnValue_t result = convertFromCUC(to, pField, from, foundLength,
|
||||
maxLength - 1);
|
||||
if (result == HasReturnvaluesIF::RETURN_OK) {
|
||||
if (foundLength != NULL) {
|
||||
*foundLength += 1;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::checkTimeOfDay(const Clock::TimeOfDay_t* time) {
|
||||
if ((time->month > 12) || (time->month == 0)) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
|
||||
if (time->day == 0) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
switch (time->month) {
|
||||
case 2:
|
||||
if (isLeapYear(time->year)) {
|
||||
if (time->day > 29) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
} else {
|
||||
if (time->day > 28) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
case 6:
|
||||
case 9:
|
||||
case 11:
|
||||
if (time->day > 30) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
if (time->day > 31) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (time->hour > 23) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
|
||||
if (time->minute > 59) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
|
||||
if (time->second > 59) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
|
||||
if (time->usecond > 999999) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertTimevalToTimeOfDay(Clock::TimeOfDay_t* to,
|
||||
timeval* from) {
|
||||
//This is rather tricky. Implement only if needed. Also, if so, move to OSAL.
|
||||
return UNSUPPORTED_TIME_FORMAT;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCDS(timeval* to, const uint8_t* from,
|
||||
size_t* foundLength, size_t maxLength) {
|
||||
uint8_t pField = *from;
|
||||
from++;
|
||||
//Check epoch
|
||||
if (pField & 0b1000) {
|
||||
return NOT_ENOUGH_INFORMATION_FOR_TARGET_FORMAT;
|
||||
}
|
||||
//Check length
|
||||
uint8_t expectedLength = 7; //Including p-Field.
|
||||
bool extendedDays = pField & 0b100;
|
||||
if (extendedDays) {
|
||||
expectedLength += 1;
|
||||
}
|
||||
if ((pField & 0b11) == 0b01) {
|
||||
expectedLength += 2;
|
||||
} else if ((pField & 0b11) == 0b10) {
|
||||
expectedLength += 4;
|
||||
}
|
||||
if (foundLength != NULL) {
|
||||
*foundLength = expectedLength;
|
||||
}
|
||||
if (expectedLength > maxLength) {
|
||||
return LENGTH_MISMATCH;
|
||||
}
|
||||
//Check and count days
|
||||
uint32_t days = 0;
|
||||
if (extendedDays) {
|
||||
days = (from[0] << 16) + (from[1] << 8) + from[2];
|
||||
from += 3;
|
||||
} else {
|
||||
days = (from[0] << 8) + from[1];
|
||||
from += 2;
|
||||
}
|
||||
//Move to POSIX epoch.
|
||||
if (days <= DAYS_CCSDS_TO_UNIX_EPOCH) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
days -= DAYS_CCSDS_TO_UNIX_EPOCH;
|
||||
to->tv_sec = days * SECONDS_PER_DAY;
|
||||
uint32_t msDay = (from[0] << 24) + (from[1] << 16) + (from[2] << 8)
|
||||
+ from[3];
|
||||
from += 4;
|
||||
to->tv_sec += (msDay / 1000);
|
||||
to->tv_usec = (msDay % 1000) * 1000;
|
||||
if ((pField & 0b11) == 0b01) {
|
||||
uint16_t usecs = (from[0] << 16) + from[1];
|
||||
from += 2;
|
||||
if (usecs > 999) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
to->tv_usec += usecs;
|
||||
} else if ((pField & 0b11) == 0b10) {
|
||||
uint32_t picosecs = (from[0] << 24) + (from[1] << 16) + (from[2] << 8)
|
||||
+ from[3];
|
||||
from += 4;
|
||||
if (picosecs > 999999) {
|
||||
return INVALID_TIME_FORMAT;
|
||||
}
|
||||
//Not very useful.
|
||||
to->tv_usec += (picosecs / 1000);
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCUC(timeval* to, uint8_t pField,
|
||||
const uint8_t* from, size_t* foundLength, size_t maxLength) {
|
||||
uint32_t secs = 0;
|
||||
uint32_t subSeconds = 0;
|
||||
uint8_t nCoarse = ((pField & 0b1100) >> 2) + 1;
|
||||
uint8_t nFine = (pField & 0b11);
|
||||
size_t totalLength = nCoarse + nFine;
|
||||
if (foundLength != NULL) {
|
||||
*foundLength = totalLength;
|
||||
}
|
||||
if (totalLength > maxLength) {
|
||||
return LENGTH_MISMATCH;
|
||||
}
|
||||
for (int count = 0; count < nCoarse; count++) {
|
||||
secs += *from << ((nCoarse * 8 - 8) * (1 + count));
|
||||
from++;
|
||||
}
|
||||
for (int count = 0; count < nFine; count++) {
|
||||
subSeconds += *from << ((nFine * 8 - 8) * (1 + count));
|
||||
from++;
|
||||
}
|
||||
//Move to POSIX epoch.
|
||||
to->tv_sec = secs;
|
||||
if (pField & 0b10000) {
|
||||
//CCSDS-Epoch
|
||||
to->tv_sec -= (DAYS_CCSDS_TO_UNIX_EPOCH * SECONDS_PER_DAY);
|
||||
}
|
||||
to->tv_usec = subsecondsToMicroseconds(subSeconds);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
uint32_t CCSDSTime::subsecondsToMicroseconds(uint16_t subseconds) {
|
||||
uint64_t temp = (uint64_t) subseconds * 1000000
|
||||
/ (1 << (sizeof(subseconds) * 8));
|
||||
return temp;
|
||||
}
|
||||
|
||||
ReturnValue_t CCSDSTime::convertFromCCS(timeval* to, const uint8_t* from,
|
||||
size_t* foundLength, size_t maxLength) {
|
||||
Clock::TimeOfDay_t tempTime;
|
||||
ReturnValue_t result = convertFromCCS(&tempTime, from, foundLength,
|
||||
maxLength);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
return Clock::convertTimeOfDayToTimeval(&tempTime, to);
|
||||
|
||||
}
|
||||
9
src/core/timemanager/CMakeLists.txt
Normal file
9
src/core/timemanager/CMakeLists.txt
Normal file
@ -0,0 +1,9 @@
|
||||
target_sources(${LIB_FSFW_NAME}
|
||||
PRIVATE
|
||||
CCSDSTime.cpp
|
||||
Countdown.cpp
|
||||
Stopwatch.cpp
|
||||
TimeMessage.cpp
|
||||
TimeStamper.cpp
|
||||
ClockCommon.cpp
|
||||
)
|
||||
57
src/core/timemanager/ClockCommon.cpp
Normal file
57
src/core/timemanager/ClockCommon.cpp
Normal file
@ -0,0 +1,57 @@
|
||||
#include "Clock.h"
|
||||
#include "../ipc/MutexGuard.h"
|
||||
|
||||
ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
|
||||
uint16_t leapSeconds;
|
||||
ReturnValue_t result = getLeapSeconds(&leapSeconds);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
timeval leapSeconds_timeval = { 0, 0 };
|
||||
leapSeconds_timeval.tv_sec = leapSeconds;
|
||||
|
||||
//initial offset between UTC and TAI
|
||||
timeval UTCtoTAI1972 = { 10, 0 };
|
||||
|
||||
timeval TAItoTT = { 32, 184000 };
|
||||
|
||||
*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
|
||||
if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
MutexGuard helper(timeMutex);
|
||||
|
||||
leapSeconds = leapSeconds_;
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
|
||||
if (timeMutex == nullptr) {
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
MutexGuard helper(timeMutex);
|
||||
|
||||
*leapSeconds_ = leapSeconds;
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t Clock::checkOrCreateClockMutex() {
|
||||
if (timeMutex == nullptr) {
|
||||
MutexFactory *mutexFactory = MutexFactory::instance();
|
||||
if (mutexFactory == nullptr) {
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
timeMutex = mutexFactory->createMutex();
|
||||
if (timeMutex == nullptr) {
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
37
src/core/timemanager/Countdown.cpp
Normal file
37
src/core/timemanager/Countdown.cpp
Normal file
@ -0,0 +1,37 @@
|
||||
#include "Countdown.h"
|
||||
|
||||
Countdown::Countdown(uint32_t initialTimeout): timeout(initialTimeout) {
|
||||
}
|
||||
|
||||
Countdown::~Countdown() {
|
||||
}
|
||||
|
||||
ReturnValue_t Countdown::setTimeout(uint32_t miliseconds) {
|
||||
ReturnValue_t return_value = Clock::getUptime( &startTime );
|
||||
timeout = miliseconds;
|
||||
return return_value;
|
||||
}
|
||||
|
||||
bool Countdown::hasTimedOut() const {
|
||||
uint32_t current_time;
|
||||
Clock::getUptime( ¤t_time );
|
||||
if ( uint32_t(current_time - startTime) >= timeout) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Countdown::isBusy() const {
|
||||
return !hasTimedOut();
|
||||
}
|
||||
|
||||
ReturnValue_t Countdown::resetTimer() {
|
||||
return setTimeout(timeout);
|
||||
}
|
||||
|
||||
void Countdown::timeOut() {
|
||||
uint32_t current_time;
|
||||
Clock::getUptime( ¤t_time );
|
||||
startTime= current_time - timeout;
|
||||
}
|
||||
74
src/core/timemanager/Stopwatch.cpp
Normal file
74
src/core/timemanager/Stopwatch.cpp
Normal file
@ -0,0 +1,74 @@
|
||||
#include "Stopwatch.h"
|
||||
#include "../serviceinterface/ServiceInterface.h"
|
||||
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
#include <iomanip>
|
||||
#endif
|
||||
|
||||
Stopwatch::Stopwatch(bool displayOnDestruction,
|
||||
StopwatchDisplayMode displayMode): displayOnDestruction(
|
||||
displayOnDestruction), displayMode(displayMode) {
|
||||
// Measures start time on initialization.
|
||||
Clock::getUptime(&startTime);
|
||||
}
|
||||
|
||||
void Stopwatch::start() {
|
||||
Clock::getUptime(&startTime);
|
||||
}
|
||||
|
||||
dur_millis_t Stopwatch::stop(bool display) {
|
||||
stopInternal();
|
||||
if(display) {
|
||||
this->display();
|
||||
}
|
||||
return elapsedTime.tv_sec * 1000 + elapsedTime.tv_usec / 1000;
|
||||
}
|
||||
|
||||
double Stopwatch::stopSeconds() {
|
||||
stopInternal();
|
||||
return timevalOperations::toDouble(elapsedTime);
|
||||
}
|
||||
|
||||
void Stopwatch::display() {
|
||||
if(displayMode == StopwatchDisplayMode::MILLIS) {
|
||||
dur_millis_t timeMillis = static_cast<dur_millis_t>(
|
||||
elapsedTime.tv_sec * 1000 + elapsedTime.tv_usec / 1000);
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "Stopwatch: Operation took " << timeMillis << " milliseconds" << std::endl;
|
||||
#else
|
||||
sif::printInfo("Stopwatch: Operation took %lu milliseconds\n\r",
|
||||
static_cast<unsigned int>(timeMillis));
|
||||
#endif
|
||||
}
|
||||
else if(displayMode == StopwatchDisplayMode::SECONDS) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info <<"Stopwatch: Operation took " << std::setprecision(3)
|
||||
<< std::fixed << timevalOperations::toDouble(elapsedTime)
|
||||
<< " seconds" << std::endl;
|
||||
#else
|
||||
sif::printInfo("Stopwatch: Operation took %.3f seconds\n\r",
|
||||
static_cast<float>(timevalOperations::toDouble(elapsedTime)));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
Stopwatch::~Stopwatch() {
|
||||
if(displayOnDestruction) {
|
||||
stopInternal();
|
||||
display();
|
||||
}
|
||||
}
|
||||
|
||||
void Stopwatch::setDisplayMode(StopwatchDisplayMode displayMode) {
|
||||
this->displayMode = displayMode;
|
||||
}
|
||||
|
||||
StopwatchDisplayMode Stopwatch::getDisplayMode() const {
|
||||
return displayMode;
|
||||
}
|
||||
|
||||
void Stopwatch::stopInternal() {
|
||||
timeval endTime;
|
||||
Clock::getUptime(&endTime);
|
||||
elapsedTime = endTime - startTime;
|
||||
}
|
||||
30
src/core/timemanager/TimeMessage.cpp
Normal file
30
src/core/timemanager/TimeMessage.cpp
Normal file
@ -0,0 +1,30 @@
|
||||
#include "TimeMessage.h"
|
||||
|
||||
TimeMessage::TimeMessage() {
|
||||
this->messageSize += sizeof(timeval) + sizeof(uint32_t);
|
||||
}
|
||||
|
||||
TimeMessage::TimeMessage(timeval setTime, uint32_t CounterValue) {
|
||||
memcpy (this->getData(), &setTime, sizeof(timeval));
|
||||
this->messageSize += sizeof(timeval) + sizeof(uint32_t);
|
||||
memcpy (this->getData() + sizeof(timeval), &CounterValue, sizeof(uint32_t));
|
||||
}
|
||||
|
||||
TimeMessage::~TimeMessage() {
|
||||
}
|
||||
|
||||
timeval TimeMessage::getTime() {
|
||||
timeval temp;
|
||||
memcpy( &temp, this->getData(), sizeof(timeval));
|
||||
return temp;
|
||||
}
|
||||
|
||||
uint32_t TimeMessage::getCounterValue() {
|
||||
uint32_t temp;
|
||||
memcpy ( &temp, this->getData() + sizeof(timeval), sizeof(uint32_t));
|
||||
return temp;
|
||||
}
|
||||
|
||||
size_t TimeMessage::getMinimumMessageSize() const {
|
||||
return this->MAX_SIZE;
|
||||
}
|
||||
23
src/core/timemanager/TimeStamper.cpp
Normal file
23
src/core/timemanager/TimeStamper.cpp
Normal file
@ -0,0 +1,23 @@
|
||||
#include "TimeStamper.h"
|
||||
#include "Clock.h"
|
||||
#include <cstring>
|
||||
|
||||
TimeStamper::TimeStamper(object_id_t objectId): SystemObject(objectId) {}
|
||||
|
||||
|
||||
ReturnValue_t TimeStamper::addTimeStamp(uint8_t* buffer,
|
||||
const uint8_t maxSize) {
|
||||
if(maxSize < TimeStamperIF::MISSION_TIMESTAMP_SIZE){
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
CCSDSTime::CDS_short cds;
|
||||
ReturnValue_t result = CCSDSTime::convertToCcsds(&cds,&now);
|
||||
if(result != HasReturnvaluesIF::RETURN_OK){
|
||||
return result;
|
||||
}
|
||||
std::memcpy(buffer,&cds,sizeof(cds));
|
||||
return result;
|
||||
}
|
||||
Reference in New Issue
Block a user