Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Gauthier Quesnel
irritator
Commits
3d255bfb
Commit
3d255bfb
authored
Jul 27, 2020
by
K-H-Ismail
Committed by
Gauthier Quesnel
Aug 11, 2020
Browse files
core: repair time_func model
parent
0f1bc139
Changes
1
Hide whitespace changes
Inline
Side-by-side
lib/include/irritator/core.hpp
View file @
3d255bfb
...
...
@@ -3384,8 +3384,8 @@ struct qss2_integrator
}
status
internal
()
noexcept
{
//printf("qss2_integrator internal\n");
{
//printf("qss2_integrator internal\n");
X
+=
u
*
sigma
+
mu
/
2.
*
sigma
*
sigma
;
q
=
X
;
...
...
@@ -3400,7 +3400,7 @@ struct qss2_integrator
status
reset
(
const
double
value_reset
)
noexcept
{
//printf("qss2_integrator reset %g\n", value_reset);
//printf("qss2_integrator reset %g\n", value_reset);
X
=
value_reset
;
q
=
X
;
...
...
@@ -3447,7 +3447,7 @@ struct qss2_integrator
message
observation
(
time
/*t*/
)
const
noexcept
{
return
message
(
X
+
u
*
sigma
+
mu
*
sigma
*
sigma
/
2.
);
return
message
(
X
);
}
};
...
...
@@ -4938,7 +4938,7 @@ struct accumulator
for
(
size_t
i
=
0
;
i
!=
PortNumber
;
++
i
)
{
auto
&
port
=
input_ports
.
get
(
x
[
i
+
PortNumber
]);
for
(
const
auto
&
msg
:
port
.
messages
)
{
irt_assert
(
msg
.
size
()
=
=
1
);
irt_assert
(
msg
.
size
()
>
=
1
);
numbers
[
i
]
=
msg
[
0
];
}
...
...
@@ -4947,7 +4947,7 @@ struct accumulator
for
(
size_t
i
=
0
;
i
!=
PortNumber
;
++
i
)
{
auto
&
port
=
input_ports
.
get
(
x
[
i
]);
for
(
const
auto
&
msg
:
port
.
messages
)
{
irt_assert
(
msg
.
size
()
=
=
1
);
irt_assert
(
msg
.
size
()
>
=
1
);
if
(
msg
[
0
]
!=
0.0
)
number
+=
numbers
[
i
];
...
...
@@ -5131,7 +5131,7 @@ struct abstract_cross
sigma
=
time_domain
<
time
>::
infinity
;
if
(
value
[
1
])
{
const
auto
wakeup
=
(
threshold
-
value
[
0
])
/
value
[
1
];
const
auto
wakeup
=
(
threshold
-
value
[
0
])
/
value
[
1
]
;
if
(
wakeup
>=
0.
)
sigma
=
wakeup
;
}
...
...
@@ -5190,133 +5190,141 @@ struct abstract_cross
//printf("delta_int t=%g e=%g sigma=%g\n", t, e, sigma);
// irt_assert(e == sigma);
}
if
(
p_if_value
.
messages
.
empty
())
{
if
constexpr
(
QssLevel
==
2
)
if_value
[
0
]
+=
if_value
[
1
]
*
e
;
if
constexpr
(
QssLevel
==
3
)
if_value
[
0
]
+=
if_value
[
1
]
*
e
+
if_value
[
2
]
*
e
*
e
;
}
else
{
for
(
const
auto
&
msg
:
p_if_value
.
messages
)
{
if_value
[
0
]
=
msg
[
0
];
//printf(" if_value %g %g\n", if_value[0], if_value[1]);
if
constexpr
(
QssLevel
>=
2
)
if_value
[
1
]
=
msg
.
size
()
>
1
?
msg
[
1
]
:
0.
;
if
(
p_if_value
.
messages
.
empty
())
{
if
constexpr
(
QssLevel
==
2
)
if_value
[
0
]
+=
if_value
[
1
]
*
e
;
if
constexpr
(
QssLevel
==
3
)
if_value
[
2
]
=
msg
.
size
()
>
2
?
msg
[
2
]
:
0.
;
if_value
[
0
]
+=
if_value
[
1
]
*
e
+
if_value
[
2
]
*
e
*
e
;
}
else
{
for
(
const
auto
&
msg
:
p_if_value
.
messages
)
{
if_value
[
0
]
=
msg
[
0
];
//printf(" if_value %g %g\n", if_value[0], if_value[1]);
if
constexpr
(
QssLevel
>=
2
)
if_value
[
1
]
=
msg
.
size
()
>
1
?
msg
[
1
]
:
0.
;
if
constexpr
(
QssLevel
==
3
)
if_value
[
2
]
=
msg
.
size
()
>
2
?
msg
[
2
]
:
0.
;
}
}
}
if
(
p_else_value
.
messages
.
empty
())
{
if
constexpr
(
QssLevel
==
2
)
else_value
[
0
]
+=
else_value
[
1
]
*
e
;
if
constexpr
(
QssLevel
==
3
)
else_value
[
0
]
+=
else_value
[
1
]
*
e
+
else_value
[
2
]
*
e
*
e
;
}
else
{
for
(
const
auto
&
msg
:
p_else_value
.
messages
)
{
else_value
[
0
]
=
msg
[
0
];
//printf(" else_value %g %g\n", else_value[0], else_value[1]);
if
constexpr
(
QssLevel
>=
2
)
else_value
[
1
]
=
msg
.
size
()
>
1
?
msg
[
1
]
:
0.
;
if
(
p_else_value
.
messages
.
empty
())
{
if
constexpr
(
QssLevel
==
2
)
else_value
[
0
]
+=
else_value
[
1
]
*
e
;
if
constexpr
(
QssLevel
==
3
)
else_value
[
2
]
=
msg
.
size
()
>
2
?
msg
[
2
]
:
0.
;
else_value
[
0
]
+=
else_value
[
1
]
*
e
+
else_value
[
2
]
*
e
*
e
;
}
else
{
for
(
const
auto
&
msg
:
p_else_value
.
messages
)
{
else_value
[
0
]
=
msg
[
0
];
//printf(" else_value %g %g\n", else_value[0], else_value[1]);
if
constexpr
(
QssLevel
>=
2
)
else_value
[
1
]
=
msg
.
size
()
>
1
?
msg
[
1
]
:
0.
;
if
constexpr
(
QssLevel
==
3
)
else_value
[
2
]
=
msg
.
size
()
>
2
?
msg
[
2
]
:
0.
;
}
}
}
if
(
p_value
.
messages
.
empty
())
{
if
constexpr
(
QssLevel
==
2
)
value
[
0
]
+=
value
[
1
]
*
e
;
if
constexpr
(
QssLevel
==
3
)
value
[
0
]
+=
value
[
1
]
*
e
+
value
[
2
]
*
e
*
e
;
}
else
{
for
(
const
auto
&
msg
:
p_value
.
messages
)
{
value
[
0
]
=
msg
[
0
];
//printf(" value %g\n", msg[0]);
if
constexpr
(
QssLevel
>=
2
)
value
[
1
]
=
msg
.
size
()
>
1
?
msg
[
1
]
:
0.
;
if
(
p_value
.
messages
.
empty
())
{
if
constexpr
(
QssLevel
==
2
)
value
[
0
]
+=
value
[
1
]
*
e
;
if
constexpr
(
QssLevel
==
3
)
value
[
2
]
=
msg
.
size
()
>
2
?
msg
[
2
]
:
0.
;
value
[
0
]
+=
value
[
1
]
*
e
+
value
[
2
]
*
e
*
e
;
}
else
{
for
(
const
auto
&
msg
:
p_value
.
messages
)
{
value
[
0
]
=
msg
[
0
];
//printf(" value %g\n", msg[0]);
if
constexpr
(
QssLevel
>=
2
)
value
[
1
]
=
msg
.
size
()
>
1
?
msg
[
1
]
:
0.
;
if
constexpr
(
QssLevel
==
3
)
value
[
2
]
=
msg
.
size
()
>
2
?
msg
[
2
]
:
0.
;
}
}
}
reach_threshold
=
false
;
reach_threshold
=
false
;
if
(
value
[
0
]
>=
threshold
)
{
last_reset
=
t
;
reach_threshold
=
true
;
sigma
=
time_domain
<
time
>::
zero
;
//printf(" need to send if_value %g\n", if_value[0]);
}
else
if
(
old_else_value
!=
else_value
[
0
])
{
sigma
=
time_domain
<
time
>::
zero
;
//printf(" need to send else_value %g\n", else_value[0]);
}
else
compute_wake_up
();
//printf(" end transition. Sigma equals %.10g (next: %.10g - %.10g\n",
// sigma,
// t + sigma,
// t + std::numeric_limits<double>::epsilon());
if
(
value
[
0
]
>=
threshold
)
{
last_reset
=
t
;
reach_threshold
=
true
;
sigma
=
time_domain
<
time
>::
zero
;
//printf(" need to send if_value %g\n", if_value[0]);
}
else
if
(
old_else_value
!=
else_value
[
0
])
{
sigma
=
time_domain
<
time
>::
zero
;
//printf(" need to send else_value %g\n", else_value[0]);
}
else
compute_wake_up
();
//printf(" end transition. Sigma equals %.10g (next: %.10g - %.10g\n",
// sigma,
// t + sigma,
// t + std::numeric_limits<double>::epsilon());
if
(
sigma
>
0.
&&
sigma
+
t
==
t
)
{
//printf("sigma + t == t with sigma > 0.!\n");
sigma
=
sigma
*
10
;
}
if
(
sigma
>
0.
&&
sigma
+
t
==
t
)
{
//printf("sigma + t == t with sigma > 0.!\n");
sigma
=
sigma
*
10
;
return
status
::
success
;
}
return
status
::
success
;
}
status
lambda
(
data_array
<
output_port
,
output_port_id
>&
output_ports
)
noexcept
{
if
(
!
block_lambda
)
{
if
constexpr
(
QssLevel
==
1
)
{
output_ports
.
get
(
y
[
o_else_value
])
.
messages
.
emplace_front
(
else_value
[
0
]);
if
(
reach_threshold
)
{
output_ports
.
get
(
y
[
o_if_value
])
.
messages
.
emplace_front
(
if_value
[
0
]);
output_ports
.
get
(
y
[
o_event
]).
messages
.
emplace_front
(
1.0
);
status
lambda
(
data_array
<
output_port
,
output_port_id
>&
output_ports
)
noexcept
{
if
(
!
block_lambda
)
{
if
constexpr
(
QssLevel
==
1
)
{
output_ports
.
get
(
y
[
o_else_value
])
.
messages
.
emplace_front
(
else_value
[
0
]);
if
(
reach_threshold
)
{
output_ports
.
get
(
y
[
o_if_value
])
.
messages
.
emplace_front
(
if_value
[
0
]);
output_ports
.
get
(
y
[
o_event
]).
messages
.
emplace_front
(
1.0
);
}
}
}
if
constexpr
(
QssLevel
==
2
)
{
output_ports
.
get
(
y
[
o_else_value
])
.
messages
.
emplace_front
(
else_value
[
0
],
else_value
[
1
]);
//printf("lambda: %.10g\n", else_value[0]);
if
(
reach_threshold
)
{
//printf("lambda reach threshold: %.10g\n", if_value[0]);
output_ports
.
get
(
y
[
o_if_value
])
.
messages
.
emplace_front
(
if_value
[
0
],
if_value
[
1
]);
output_ports
.
get
(
y
[
o_event
]).
messages
.
emplace_front
(
1.0
);
if
constexpr
(
QssLevel
==
2
)
{
output_ports
.
get
(
y
[
o_else_value
])
.
messages
.
emplace_front
(
else_value
[
0
],
else_value
[
1
]);
//printf("lambda: %.10g\n", else_value[0]);
if
(
reach_threshold
)
{
//printf("lambda reach threshold: %.10g\n", if_value[0]);
output_ports
.
get
(
y
[
o_if_value
])
.
messages
.
emplace_front
(
if_value
[
0
],
if_value
[
1
]);
output_ports
.
get
(
y
[
o_event
]).
messages
.
emplace_front
(
1.0
);
}
}
}
if
constexpr
(
QssLevel
==
3
)
{
output_ports
.
get
(
y
[
o_else_value
])
.
messages
.
emplace_front
(
else_value
[
0
],
else_value
[
1
],
else_value
[
2
]);
if
(
reach_threshold
)
{
output_ports
.
get
(
y
[
o_if_value
])
.
messages
.
emplace_front
(
if_value
[
0
],
if_value
[
1
],
if_value
[
2
]);
output_ports
.
get
(
y
[
o_event
]).
messages
.
emplace_front
(
1.0
);
if
constexpr
(
QssLevel
==
3
)
{
output_ports
.
get
(
y
[
o_else_value
])
.
messages
.
emplace_front
(
else_value
[
0
],
else_value
[
1
],
else_value
[
2
]);
if
(
reach_threshold
)
{
output_ports
.
get
(
y
[
o_if_value
])
.
messages
.
emplace_front
(
if_value
[
0
],
if_value
[
1
],
if_value
[
2
]);
output_ports
.
get
(
y
[
o_event
]).
messages
.
emplace_front
(
1.0
);
}
}
}
}
return
status
::
success
;
}
return
status
::
success
;
}
message
observation
(
time
/*t*/
)
const
noexcept
{
return
message
(
value
[
0
],
if_value
[
0
],
else_value
[
0
]);
}
};
message
observation
(
time
/*t*/
)
const
noexcept
{
return
message
(
value
[
0
],
if_value
[
0
],
else_value
[
0
]);
}
};
using
qss1_cross
=
abstract_cross
<
1
>
;
using
qss2_cross
=
abstract_cross
<
2
>
;
using
qss3_cross
=
abstract_cross
<
3
>
;
inline
double
sin_time_function
(
double
t
)
noexcept
{
const
double
f0
=
0.1
;
const
double
pi
=
std
::
acos
(
-
1
);
return
std
::
sin
(
2
*
pi
*
f0
*
t
);
}
inline
double
square_time_function
(
double
t
)
noexcept
{
...
...
@@ -5335,6 +5343,8 @@ struct time_func
output_port_id
y
[
1
];
time
sigma
;
double
default_sigma
=
0.01
;
double
(
*
default_f
)(
double
)
=
&
time_function
;
double
value
;
...
...
@@ -5343,8 +5353,8 @@ struct time_func
status
initialize
(
data_array
<
message
,
message_id
>&
/*init*/
)
noexcept
{
f
=
default_f
;
sigma
=
1.0
;
value
=
sigma
;
sigma
=
default_sigma
;
value
=
0.0
;
return
status
::
success
;
}
...
...
@@ -5353,10 +5363,7 @@ struct time_func
time
/*e*/
,
time
/*r*/
)
noexcept
{
sigma
=
(
*
f
)(
t
);
value
=
sigma
;
value
=
(
*
f
)(
t
);
return
status
::
success
;
}
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment