enum control_state StateMachine::get_state() {
if(ToSeconds(SubtractTime(GetTime(), com_stamp)) > com_duration) {
state = kStop;
}
else if ( (state == kQuat || state == kOpen || state == kOpenAttitude)
&& (ToSeconds(SubtractTime(GetTime(), pilot_stamp)) > pilot_duration) ) {
standby_needs_init = true;
state = kStandby;
}
return state;
}
//This function expires a cookie for a ScreenName by setting its expires
// parameter to 1 hour in the past
void ExpireCookie(void)
{
long ltime;
struct tm *GMTime;
//19 = "Set-Cookie: " + " Facts="
char szCookieParm[MAXNAMLEN + MAX_SCREEN_NAME_SIZE + 19 + 1];
char szCookieTime[MAX_COOKIE_TIME_SIZE + 1];
strcpy(szCookieParm, "Set-Cookie: ");
strcat(szCookieParm, g_szScreenName);
strcat(szCookieParm, " Facts=");
strcat(szCookieParm, g_szGetCookiesFile);
strcat(szCookieParm, "; ");
//Get the current time
time(<ime);
//subtract 1 hour
ltime -= ToSeconds(0L, 1, 0, 0);
//Convert to GMT/UTC time
GMTime = localtime(<ime);
//Format for the cookie
memset(szCookieTime, '\0', sizeof(szCookieTime) + 1);
strftime(szCookieTime, MAX_COOKIE_TIME_SIZE, "expires=%A, %d-%b-%Y %H:%M:%S GMT; ", GMTime);
//Append it to current cookie contents and print it.
strcat(szCookieParm, szCookieTime); //append the GMT time
printf("%s\n", szCookieParm);
}
// Update outputs based on desired and estimated state
void PulsingCoaxControllerQuat::Update() {
struct Time new_t = GetTime();
float dt = ToSeconds(SubtractTime(new_t, t));
t = new_t;
// collect estimated state
QuatState est;
est.q = ukf->ukf.get_q();
est.w = ukf->ukf.get_w();
// calculate PD control law
float omega = 0; // basis for angular momentum estimate
float omega_filt = BiquadLpfDynamicCUpdate(&omega_filter, omega, dt);
pd->Calculate(dt, thrust_des, des, est, omega_filt, u);
// saturate thrust command
u.thrust = SatFloat(u.thrust, -thrust_sat, thrust_sat);
// saturate yaw command
u.yaw = SatFloat(u.yaw, -u.thrust, u.thrust);
u.yaw = SatFloat(u.yaw, -yaw_sat, yaw_sat);
// calculate mean motor voltages
top_mean = u.thrust - u.yaw;
if(enable_bottom)
bottom_mean = u.thrust + u.yaw;
else
bottom_mean = 0;
// map roll and pitch to motor pulsing commands
u_phase = atan2f(u.pitch, u.roll);
top_pulse_amp = sqrtf(u.roll*u.roll + u.pitch*u.pitch);
if(top_pulse_amp < pulse_thresh) {
top_pulse_amp = 0;
}
else {
top_pulse_amp = top_pulse_amp + pulse_min;
}
// amplitude saturation
top_pulse_amp = fmin(top_pulse_amp, pulse_sat); // no pulse above sat value
// phase correction
top_pulse_phase = WrapRad(- u_phase + advance_angle);
// zero pulsing amplitude if top rotor mean drive is too small
if(top_mean < 1.0f) {
top_pulse_amp = 0;
}
}
void UavReporter::FillMsgUavReporterEstimator(MsgUavReporterEstimator *tx_msg) {
Quaternionf q; q = est->ukf.get_q();
Vector3f w; w = est->ukf.get_w();
tx_msg->t = ToSeconds(imu->time);
tx_msg->wx_meas = imu->w[0];
tx_msg->wy_meas = imu->w[1];
tx_msg->wz_meas = imu->w[2];
tx_msg->ax_meas = imu->a[0];
tx_msg->ay_meas = imu->a[1];
tx_msg->az_meas = imu->a[2];
tx_msg->wx_est = w(0);
tx_msg->wy_est = w(1);
tx_msg->wz_est = w(2);
tx_msg->qw_est = q.w();
tx_msg->qx_est = q.x();
tx_msg->qy_est = q.y();
tx_msg->qz_est = q.z();
}
double
TimeDuration::ToSecondsSigDigits() const
{
return ToSeconds();
}
MFBT_API double
BaseTimeDurationPlatformUtils::ToSecondsSigDigits(int64_t aTicks)
{
return ToSeconds(aTicks);
}
time_t Time2time_t(Time x) { return ToSeconds(x).count(); }
void StateMachine::ReadMsg(CommunicationInterface& com, uint8_t* rx_data, uint8_t rx_length) {
(void)com; // intentionally unused
(void)rx_length; // intentionally unused
uint8_t type = rx_data[0];
// message received, reset communications timeout
com_stamp = GetTime();
// transition based on current state
if(state == kStop) {
if(type == kTypeQuatPilot || type == kTypeQuatFullObsPilot) {
pilot_stamp = GetTime();
state = kQuat;
}
else if(type == kTypeOpenPilot) {
pilot_stamp = GetTime();
state = kOpen;
}
else if(type == kTypeOpenAttitudePilot) {
pilot_stamp = GetTime();
state = kOpenAttitude;
}
else {
standby_needs_init = false;
state = kStandby;
}
}
else if(state == kStandby) {
if(type == kTypeQuatPilot || type == kTypeQuatFullObsPilot) {
pilot_stamp = GetTime();
state = kQuat;
}
else if(type == kTypeOpenPilot) {
pilot_stamp = GetTime();
state = kOpen;
}
else if(type == kTypeOpenAttitudePilot) {
pilot_stamp = GetTime();
state = kOpenAttitude;
}
else if(type == kTypeKill) {
state = kStop;
}
}
else if(state == kQuat || state == kOpen || state == kOpenAttitude) {
if(type == kTypeQuatPilot || type == kTypeQuatFullObsPilot) {
pilot_stamp = GetTime();
state = kQuat;
}
else if(type == kTypeOpenPilot) {
pilot_stamp = GetTime();
state = kOpen;
}
else if(type == kTypeOpenAttitudePilot) {
pilot_stamp = GetTime();
state = kOpenAttitude;
}
else if(type == kTypeKill) {
state = kStop;
}
else if (ToSeconds(SubtractTime(GetTime(), pilot_stamp)) > pilot_duration) {
standby_needs_init = true;
state = kStandby;
}
}
}
float StopWatch::ElapsedSeconds() const
{
return mEnabled ? ToSeconds(mElapsedTime) : 0.f;
}
time_t Convert( time_t Month, time_t Day, time_t Year, time_t Hours, time_t Minutes, time_t Seconds, MERIDIAN Meridian, DSTMODE dst )
{
static int DaysNormal[13] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
static int DaysLeap[13] = { 0, 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
static int LeapYears[17] = { 1972, 1976, 1980, 1984, 1988, 1992, 1996, 2000, 2004, 2008, 2012, 2016, 2020, 2024, 2028, 2032, 2036 };
int *yp;
int *mp;
int i;
time_t Julian;
time_t tod;
if ( Year < 0 )
Year = 0 - Year;
if ( Year <= 69 )
Year += 2000;
if ( Year <= 99 )
Year += 1900;
if ( Year <= 1969 )
Year += 100;
mp = DaysNormal;
yp = LeapYears;
for ( ; yp < ""; yp++ )
{
if ( yp[0] == Year )
{
mp = DaysLeap;
break;
}
else
{
// yp++;
}
}
if ( Year <= 1969 || Year > 2038 || Month <= 0 || Month > 12 || Day <= 0 || mp[ Month ] < Day )
{
return -1;
}
Julian = Day + ( Year * 365 ) + -719050 + -1;
yp = LeapYears;
for ( ; yp < "" && yp[0] < Year; Julian++ )
{
yp++;
// Julian++;
}
i = 1;
for ( ; i < Month; i++ )
{
mp++;
Julian += mp[0];
// i++;
}
Julian *= 0x15180;
Julian += yyTimezone * 60;
tod = ToSeconds( Hours, Minutes, Seconds, Meridian );
if ( tod < 0 )
{
return -1;
}
Julian += tod;
tod = Julian;
if ( dst == DSTon || ( dst == DSTmaybe && *(int*)localtime( &tod )/*.32*/ ) )
Julian += -3600;
return Julian;
}
int ProcessCookies(void)
{
char szKeepFacts[7], *szFileName;
char szTargetScreen[MAX_SCREEN_NAME_SIZE + 1], *p;
long ltime, lDays;
struct tm *GMTime;
int iHours, iMins;
int bResult;
//19 = "Set-Cookie: " + " Facts="
char szCookieParm[MAXNAMLEN + MAX_SCREEN_NAME_SIZE + 19 + 1];
//7 = "Date | "
char szCookieExpireParm[MAX_COOKIE_TIME_SIZE + 7 + 1];
char szCookieTime[MAX_COOKIE_TIME_SIZE + 1];
//Determine if the screen wants to preserve facts using the
// 'cookie' option
GetWebCLIPSSettings(
g_szScreenName, // section name
"PreserveFacts", // entry name
"no", // default value
szKeepFacts, // return value
sizeof(szKeepFacts), // max length
g_szWebCLIPSINI
);
//No cookies requested, get out
if(strcasecmp(szKeepFacts, "cookie") != 0)
return(0);
//Determine which screen the facts are being saved for
GetWebCLIPSSettings(
g_szScreenName, // section name
"SaveFactsFor", // entry name
"", // default value
szTargetScreen, // return value
sizeof(szTargetScreen), // max length
g_szWebCLIPSINI
);
if(strlen(szTargetScreen) == 0)
{
PrintMIMEHeader();
ProcessErrorCode("COOK0001", g_szScreenName, 'n', 'n');
return(-1);
}
//Append ' Facts'. This is where the target screen will look to
// get the name of the file to load.
strcpy(szCookieParm, "Set-Cookie: ");
strcat(szCookieParm, szTargetScreen);
strcat(szCookieParm, " Facts=");
//Get a unique filename to store facts
bResult = GenerateTempFileName(&szFileName);
if(bResult == FALSE)
{
ProcessErrorCode("COOK0002", g_szScreenName, 'n', 'n');
return(-1);
}
else
{
//Copy the file for the PreserveFacts function call
strcpy(g_szSaveCookiesFile, szFileName);
//Add the filename to the CookieParm string
strcat(szCookieParm, szFileName);
strcat(szCookieParm, "; "); //Append a semicolon and a space
free(szFileName);
}
//Get any screen-specific cookie expiration information
GetWebCLIPSSettings(
g_szScreenName, // section name
"CookieExpiration", // entry name
"", // default value
szCookieExpireParm, // return value
sizeof(szCookieExpireParm), // max length
g_szWebCLIPSINI
);
//If there is none then check to see if there is any
// default expiration information for the cookie
if(strlen(szCookieExpireParm) == 0)
{
GetWebCLIPSSettings(
"System", // section name
"CookieExpiration", // entry name
"", // default value
szCookieExpireParm, // return value
sizeof(szCookieExpireParm), // max length
g_szWebCLIPSINI
);
}
//If there is still no expiration info found then the cookie will
// be written without an 'expires'. This will have the effect that
// the cookie will be present until the end of the session. This
// means the cookie will be present until IE or NN is stopped/closed
// terminated ... whatever.
if(strlen(szCookieExpireParm) == 0)
{
printf("%s\n", szCookieParm);
return(0);
}
//We have an expiration parameter. Check to see if it is of the
// type : Date | 'Standard Cookie expiration'. If we find a '|'
// then we assume that anything to the right of it is a properly
// formed 'Standard Cookie expiration' as given in the document :
// http://home.netscape.com/newsref/std/cookie_spec.html.
p = strtok(szCookieExpireParm, " \t");
if(strcasecmp(p, "date") == 0)
{
p = strchr(p, NULL);
p++; //Get past NULL
p = strchr(p, '|'); //Up to the pipe ( '|' )
p++; //Past the pipe
while(!isalpha(*p)) //Past the white space
p++;
strcat(szCookieParm, "expires=");
strcat(szCookieParm, p);
strcat(szCookieParm, "; "); //append a semicolon and a space
printf("%s\n", szCookieParm);
return(0);
}
//If we are here then the expires parameter is of the
// form Days, Hours, Mins. This will be used to modify the
// current time (GMT) and format an expiration date.
//Construct a CTimeSpan object given the entry in WebCLIPS.INI
//Days parameter first
p = strtok(szCookieExpireParm, ",");
lDays = atol(p);
//Hours next
p = strtok(NULL, ",");
iHours = atoi(p);
//Minutes next
p = strtok(NULL, ",");
iMins = atoi(p);
//Get the current time
time(<ime);
//Add Days, Hours, Mins in terms of seconds to ltime
ltime += ToSeconds(lDays, iHours, iMins, 0);
//Convert to GMT/UTC time
GMTime = localtime(<ime);
//Clear the buffer
memset(szCookieTime, '\0', sizeof(szCookieTime) + 1);
strftime(szCookieTime, MAX_COOKIE_TIME_SIZE, "expires=%A, %d-%b-%Y %H:%M:%S GMT; ", GMTime);
//Append it to current cookie contents and print it.
strcat(szCookieParm, szCookieTime); //append the GMT time
printf("%s\n", szCookieParm);
return(0);
}
