/* Parse replicator site element */
static int _add_site(struct lv_segment *seg,
const char *key,
const struct dm_config_node *sn)
{
struct dm_pool *mem = seg->lv->vg->vgmem;
const struct dm_config_node *cn;
struct replicator_site *rsite;
if (!(rsite = _get_site(seg->lv, key)))
return_0;
if (!dm_config_find_node(sn, "site_index"))
return SEG_LOG_ERROR("Mandatory site_index is missing for");
rsite->state = _get_state(sn, "state", REPLICATOR_STATE_PASSIVE);
rsite->site_index = _get_config_uint32(sn, "site_index", 0);
if (rsite->site_index > seg->rsite_index_highest)
return SEG_LOG_ERROR("site_index=%d > highest_site_index=%d for",
rsite->site_index, seg->rsite_index_highest);
rsite->fall_behind_data = _get_config_uint64(sn, "fall_behind_data", 0);
rsite->fall_behind_ios = _get_config_uint32(sn, "fall_behind_ios", 0);
rsite->fall_behind_timeout = _get_config_uint32(sn, "fall_behind_timeout", 0);
rsite->op_mode = DM_REPLICATOR_SYNC;
if (rsite->fall_behind_data ||
rsite->fall_behind_ios ||
rsite->fall_behind_timeout) {
if (rsite->fall_behind_data && rsite->fall_behind_ios)
return SEG_LOG_ERROR("Defined both fall_behind_data "
"and fall_behind_ios in");
if (rsite->fall_behind_data && rsite->fall_behind_timeout)
return SEG_LOG_ERROR("Defined both fall_behind_data "
"and fall_behind_timeout in");
if (rsite->fall_behind_ios && rsite->fall_behind_timeout)
return SEG_LOG_ERROR("Defined both fall_behind_ios "
"and fall_behind_timeout in");
rsite->op_mode = _get_op_mode(sn, "operation_mode",
rsite->op_mode);
}
if ((cn = dm_config_find_node(sn, "volume_group"))) {
if (!cn->v || cn->v->type != DM_CFG_STRING)
return SEG_LOG_ERROR("volume_group must be a string in");
if (!(rsite->vg_name = dm_pool_strdup(mem, cn->v->v.str)))
return_0;
} else if (rsite->site_index != 0)
return SEG_LOG_ERROR("volume_group is mandatory for remote site in");
return 1;
}
int _get(ng_netdev_t *dev, netopt_t opt, void *value, size_t max_len)
{
(void)dev;
switch (opt) {
case NETOPT_ADDRESS:
return _get_address(value, max_len);
case NETOPT_CHANNEL:
return _get_channel(value, max_len);
case NETOPT_NID:
return _get_pan(value, max_len);
case NETOPT_TX_POWER:
return _get_txpower(value, max_len);
case NETOPT_STATE:
return _get_state(value, max_len);
default:
return -ENOTSUP;
}
}
static int _get(netdev2_t *netdev, netopt_t opt, void *val, size_t max_len)
{
at86rf2xx_t *dev = (at86rf2xx_t *) netdev;
if (netdev == NULL) {
return -ENODEV;
}
/* getting these options doesn't require the transceiver to be responsive */
switch (opt) {
case NETOPT_CHANNEL_PAGE:
if (max_len < sizeof(uint16_t)) {
return -EOVERFLOW;
}
((uint8_t *)val)[1] = 0;
((uint8_t *)val)[0] = at86rf2xx_get_page(dev);
return sizeof(uint16_t);
case NETOPT_MAX_PACKET_SIZE:
if (max_len < sizeof(int16_t)) {
return -EOVERFLOW;
}
*((uint16_t *)val) = AT86RF2XX_MAX_PKT_LENGTH - _MAX_MHR_OVERHEAD;
return sizeof(uint16_t);
case NETOPT_STATE:
if (max_len < sizeof(netopt_state_t)) {
return -EOVERFLOW;
}
*((netopt_state_t *)val) = _get_state(dev);
return sizeof(netopt_state_t);
case NETOPT_PRELOADING:
if (dev->netdev.flags & AT86RF2XX_OPT_PRELOADING) {
*((netopt_enable_t *)val) = NETOPT_ENABLE;
}
else {
*((netopt_enable_t *)val) = NETOPT_DISABLE;
}
return sizeof(netopt_enable_t);
case NETOPT_PROMISCUOUSMODE:
if (dev->netdev.flags & AT86RF2XX_OPT_PROMISCUOUS) {
*((netopt_enable_t *)val) = NETOPT_ENABLE;
}
else {
*((netopt_enable_t *)val) = NETOPT_DISABLE;
}
return sizeof(netopt_enable_t);
case NETOPT_RX_START_IRQ:
*((netopt_enable_t *)val) =
!!(dev->netdev.flags & AT86RF2XX_OPT_TELL_RX_START);
return sizeof(netopt_enable_t);
case NETOPT_RX_END_IRQ:
*((netopt_enable_t *)val) =
!!(dev->netdev.flags & AT86RF2XX_OPT_TELL_RX_END);
return sizeof(netopt_enable_t);
case NETOPT_TX_START_IRQ:
*((netopt_enable_t *)val) =
!!(dev->netdev.flags & AT86RF2XX_OPT_TELL_TX_START);
return sizeof(netopt_enable_t);
case NETOPT_TX_END_IRQ:
*((netopt_enable_t *)val) =
!!(dev->netdev.flags & AT86RF2XX_OPT_TELL_TX_END);
return sizeof(netopt_enable_t);
case NETOPT_CSMA:
*((netopt_enable_t *)val) =
!!(dev->netdev.flags & AT86RF2XX_OPT_CSMA);
return sizeof(netopt_enable_t);
default:
/* Can still be handled in second switch */
break;
}
int res;
if (((res = netdev2_ieee802154_get((netdev2_ieee802154_t *)netdev, opt, val,
max_len)) >= 0) || (res != -ENOTSUP)) {
return res;
}
uint8_t old_state = at86rf2xx_get_status(dev);
res = 0;
/* temporarily wake up if sleeping */
if (old_state == AT86RF2XX_STATE_SLEEP) {
at86rf2xx_assert_awake(dev);
}
/* these options require the transceiver to be not sleeping*/
switch (opt) {
case NETOPT_TX_POWER:
if (max_len < sizeof(int16_t)) {
res = -EOVERFLOW;
}
else {
*((uint16_t *)val) = at86rf2xx_get_txpower(dev);
res = sizeof(uint16_t);
}
break;
case NETOPT_RETRANS:
if (max_len < sizeof(uint8_t)) {
res = -EOVERFLOW;
}
else {
*((uint8_t *)val) = at86rf2xx_get_max_retries(dev);
res = sizeof(uint8_t);
}
break;
case NETOPT_IS_CHANNEL_CLR:
if (at86rf2xx_cca(dev)) {
*((netopt_enable_t *)val) = NETOPT_ENABLE;
}
else {
*((netopt_enable_t *)val) = NETOPT_DISABLE;
}
res = sizeof(netopt_enable_t);
break;
case NETOPT_CSMA_RETRIES:
if (max_len < sizeof(uint8_t)) {
res = -EOVERFLOW;
}
else {
*((uint8_t *)val) = at86rf2xx_get_csma_max_retries(dev);
res = sizeof(uint8_t);
}
break;
case NETOPT_CCA_THRESHOLD:
if (max_len < sizeof(int8_t)) {
res = -EOVERFLOW;
}
else {
*((int8_t *)val) = at86rf2xx_get_cca_threshold(dev);
res = sizeof(int8_t);
}
break;
default:
res = -ENOTSUP;
}
/* go back to sleep if were sleeping */
if (old_state == AT86RF2XX_STATE_SLEEP) {
at86rf2xx_set_state(dev, AT86RF2XX_STATE_SLEEP);
}
return res;
}
static ipv6_addr_t *_src_addr_selection(gnrc_netif_t *netif,
const ipv6_addr_t *dst,
uint8_t *candidate_set)
{
/* create temporary set for assigning "points" to candidates winning in the
* corresponding rules.
*/
uint8_t winner_set[GNRC_NETIF_IPV6_ADDRS_NUMOF];
memset(winner_set, 0, GNRC_NETIF_IPV6_ADDRS_NUMOF);
uint8_t max_pts = 0;
/* _create_candidate_set() assures that `dst` is not unspecified and if
* `dst` is loopback rule 1 will fire anyway. */
uint8_t dst_scope = _get_scope(dst);
DEBUG("finding the best match within the source address candidates\n");
for (unsigned i = 0; i < GNRC_NETIF_IPV6_ADDRS_NUMOF; i++) {
ipv6_addr_t *ptr = &(netif->ipv6.addrs[i]);
DEBUG("Checking address: %s\n",
ipv6_addr_to_str(addr_str, ptr, sizeof(addr_str)));
/* entries which are not part of the candidate set can be ignored */
if (!(bf_isset(candidate_set, i))) {
DEBUG("Not part of the candidate set - skipping\n");
continue;
}
/* Rule 1: if we have an address configured that equals the destination
* use this one as source */
if (ipv6_addr_equal(ptr, dst)) {
DEBUG("Ease one - rule 1\n");
return ptr;
}
/* Rule 2: Prefer appropriate scope. */
/* both link local */
uint8_t candidate_scope = _get_scope(ptr);
if (candidate_scope == dst_scope) {
DEBUG("winner for rule 2 (same scope) found\n");
winner_set[i] += RULE_2A_PTS;
if (winner_set[i] > max_pts) {
max_pts = RULE_2A_PTS;
}
}
else if (candidate_scope < dst_scope) {
DEBUG("winner for rule 2 (smaller scope) found\n");
winner_set[i] += RULE_2B_PTS;
if (winner_set[i] > max_pts) {
max_pts = winner_set[i];
}
}
/* Rule 3: Avoid deprecated addresses. */
if (_get_state(netif, i) == GNRC_NETIF_IPV6_ADDRS_FLAGS_STATE_DEPRECATED) {
DEBUG("winner for rule 3 found\n");
winner_set[i] += RULE_3_PTS;
if (winner_set[i] > max_pts) {
max_pts = winner_set[i];
}
}
/* Rule 4: Prefer home addresses.
* Does not apply, gnrc does not support Mobile IP.
* TODO: update as soon as gnrc supports Mobile IP
*/
/* Rule 5: Prefer outgoing interface.
* RFC 6724 says:
* "It is RECOMMENDED that the candidate source addresses be the set of
* unicast addresses assigned to the interface that will be used to
* send to the destination (the "outgoing" interface). On routers,
* the candidate set MAY include unicast addresses assigned to any
* interface that forwards packets, subject to the restrictions
* described below."
* Currently this implementation uses ALWAYS source addresses assigned
* to the outgoing interface. Hence, Rule 5 is always fulfilled.
*/
/* Rule 6: Prefer matching label.
* Flow labels are currently not supported by gnrc.
* TODO: update as soon as gnrc supports flow labels
*/
/* Rule 7: Prefer temporary addresses.
* Temporary addresses are currently not supported by gnrc.
* TODO: update as soon as gnrc supports temporary addresses
*/
}
/* reset candidate set to mark winners */
memset(candidate_set, 0, (GNRC_NETIF_IPV6_ADDRS_NUMOF + 7) / 8);
/* check if we have a clear winner */
/* collect candidates with maximum points */
for (int i = 0; i < GNRC_NETIF_IPV6_ADDRS_NUMOF; i++) {
if (winner_set[i] == max_pts) {
bf_set(candidate_set, i);
}
}
/* otherwise apply rule 8: Use longest matching prefix. */
int res;
_match(netif, dst, candidate_set, &res);
return (res < 0) ? NULL : &netif->ipv6.addrs[res];
}
int gnrc_netif_ipv6_addr_add_internal(gnrc_netif_t *netif,
const ipv6_addr_t *addr,
unsigned pfx_len, uint8_t flags)
{
unsigned idx = UINT_MAX;
assert((netif != NULL) && (addr != NULL));
assert(!(ipv6_addr_is_multicast(addr) || ipv6_addr_is_unspecified(addr) ||
ipv6_addr_is_loopback(addr)));
assert((pfx_len > 0) && (pfx_len <= 128));
gnrc_netif_acquire(netif);
if ((flags & GNRC_NETIF_IPV6_ADDRS_FLAGS_STATE_MASK) ==
GNRC_NETIF_IPV6_ADDRS_FLAGS_STATE_TENTATIVE) {
/* set to first retransmission */
flags &= ~GNRC_NETIF_IPV6_ADDRS_FLAGS_STATE_TENTATIVE;
flags |= 0x1;
}
for (unsigned i = 0; i < GNRC_NETIF_IPV6_ADDRS_NUMOF; i++) {
if (ipv6_addr_equal(&netif->ipv6.addrs[i], addr)) {
gnrc_netif_release(netif);
return i;
}
if ((idx == UINT_MAX) && (netif->ipv6.addrs_flags[i] == 0)) {
idx = i;
}
}
if (idx == UINT_MAX) {
gnrc_netif_release(netif);
return -ENOMEM;
}
netif->ipv6.addrs_flags[idx] = flags;
memcpy(&netif->ipv6.addrs[idx], addr, sizeof(netif->ipv6.addrs[idx]));
#ifdef MODULE_GNRC_IPV6_NIB
#if GNRC_IPV6_NIB_CONF_ARSM
ipv6_addr_t sol_nodes;
int res;
/* TODO: SHOULD delay join between 0 and MAX_RTR_SOLICITATION_DELAY
* for SLAAC */
ipv6_addr_set_solicited_nodes(&sol_nodes, addr);
res = gnrc_netif_ipv6_group_join_internal(netif, &sol_nodes);
if (res < 0) {
DEBUG("nib: Can't join solicited-nodes of %s on interface %u\n",
ipv6_addr_to_str(addr_str, addr, sizeof(addr_str)),
netif->pid);
}
#endif /* GNRC_IPV6_NIB_CONF_ARSM */
if (_get_state(netif, idx) == GNRC_NETIF_IPV6_ADDRS_FLAGS_STATE_VALID) {
void *state = NULL;
gnrc_ipv6_nib_pl_t ple;
bool in_pl = false;
while (gnrc_ipv6_nib_pl_iter(netif->pid, &state, &ple)) {
if (ipv6_addr_match_prefix(&ple.pfx, addr) >= pfx_len) {
in_pl = true;
}
}
if (!in_pl) {
gnrc_ipv6_nib_pl_set(netif->pid, addr, pfx_len,
UINT32_MAX, UINT32_MAX);
}
}
#if GNRC_IPV6_NIB_CONF_SLAAC
else {
/* TODO: send out NS to solicited nodes for DAD probing */
}
#endif
#else
(void)pfx_len;
#endif
gnrc_netif_release(netif);
return idx;
}
void Simu::_make_robot_init(float duration)
{
robot_t rob = this->robot();
Eigen::Vector3d rot = rob->rot();
_arrival_angle = atan2(cos(rot[2]) * sin(rot[1]) * sin(rot[0]) + sin(rot[2]) * cos(rot[0]), cos(rot[2]) * cos(rot[1])) * 180 / M_PI;
//std::cout<<rot[0]<< " " << rot[1] <<" "<<rot[2]<<std::endl;
// std::cout<<"initial angle "<<_arrival_angle<<std::endl;
Eigen::Vector3d target_pos(0, 2, 0.2);
_controller.moveRobot(rob, 0);
/* float t = 0.0f;
while (t < 1)
{
t += step;
next_step();
}*/
/*if(!stabilize_robot())
{
_energy=1e20;
_covered_distance=1e10;
std::cout<<"non stab"<<std::endl;
return;
}*/
Eigen::VectorXd act_state = _get_state(rob);
float t = 0;
int index = 0;
#ifdef GRAPHIC
while (t < duration && !_visitor.done())
#else
while (t < duration)
#endif
{
_controller.moveRobot(rob, t);
Eigen::VectorXd new_state = _get_state(rob);
_energy += (act_state - new_state).array().abs().sum();
act_state = new_state;
if (_robot->bodies()[0]->get_in_contact() || _env->get_colision_between_legs()) {
#ifdef GRAPHIC
std::cout << "mort subite" << std::endl;
#endif
std::cout << "mort subite" << std::endl;
//Eigen::Vector3d next_pos = rob->pos();
//_covered_distance = round(next_pos[1]*100) / 100.0f;
_covered_distance = 00.0f;
//_covered_distance=-10002;
return;
}
int nbCassee = 0;
if (index % 2 == 0)
for (unsigned i = 0; i < 6; ++i) {
switch (i) {
case 0:
if (_controller.isBroken(i)) {
_behavior_contact_0.push_back(0);
nbCassee++;
}
else {
_behavior_contact_0.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 1:
if (_controller.isBroken(i)) {
_behavior_contact_1.push_back(0);
nbCassee++;
}
else {
_behavior_contact_1.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 2:
if (_controller.isBroken(i)) {
_behavior_contact_2.push_back(0);
nbCassee++;
}
else {
_behavior_contact_2.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 3:
if (_controller.isBroken(i)) {
_behavior_contact_3.push_back(0);
nbCassee++;
}
else {
_behavior_contact_3.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 4:
if (_controller.isBroken(i)) {
_behavior_contact_4.push_back(0);
nbCassee++;
}
else {
_behavior_contact_4.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 5:
if (_controller.isBroken(i)) {
_behavior_contact_5.push_back(0);
nbCassee++;
}
else {
_behavior_contact_5.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
}
}
_behavior_traj.push_back(rob->pos());
t += step;
next_step();
++index;
}
if (fabs(_angle) < 0.01) {
stabilize_robot();
}
Eigen::Vector3d next_pos = rob->pos();
// _covered_distance=sqrt((next_pos[0] - target_pos[0])*(next_pos[0] - target_pos[0])+(next_pos[1] - target_pos[1])*(next_pos[1] - target_pos[1])+(next_pos[2] - target_pos[2])*(next_pos[2] - target_pos[2]));
// _covered_distance=2-_covered_distance;
// _covered_distance = round(next_pos[1]*100) / 100.0f;
_final_pos.resize(2);
_final_pos[0] = next_pos[0];
_final_pos[1] = next_pos[1];
// _covered_distance = round(sqrt(next_pos[0]*next_pos[0]+next_pos[1]*next_pos[1]+next_pos[2]*next_pos[2])*100) / 100.0f;
_covered_distance = round(next_pos[1] * 100) / 100.0f;
if (fabs(_covered_distance) > 10) {
_covered_distance = 00.0f;
}
_direction = atan2(-next_pos[0], next_pos[1]) * 180 / M_PI;
rot = rob->rot();
_arrival_angle = atan2(cos(rot[2]) * sin(rot[1]) * sin(rot[0]) + sin(rot[2]) * cos(rot[0]), cos(rot[2]) * cos(rot[1])) * 180 / M_PI;
while (_arrival_angle < -180)
_arrival_angle += 360;
while (_arrival_angle > 180)
_arrival_angle -= 360;
//std::cout<<"final angle "<<_arrival_angle<<" déviation: "<<_direction-_arrival_angle<<" fit: "<<_covered_distance<<std::endl;
//std::cout<<rot[0]<< " " << rot[1] <<" "<<rot[2]<<std::endl;
if (_transf) {
std::vector<std::vector<float>> contacts;
contacts.push_back(_behavior_contact_0);
contacts.push_back(_behavior_contact_1);
contacts.push_back(_behavior_contact_2);
contacts.push_back(_behavior_contact_3);
contacts.push_back(_behavior_contact_4);
contacts.push_back(_behavior_contact_5);
write_data(contacts, "contacts");
std::vector<float> angles;
angles.push_back(rob->rot()[0]);
angles.push_back(rob->rot()[1]);
angles.push_back(rob->rot()[2]);
write_data(angles, "angles");
write_data(_covered_distance, "fit");
}
}
int _get(netdev_t *netdev, netopt_t opt, void *value, size_t len)
{
kw2xrf_t *dev = (kw2xrf_t *)netdev;
if (dev == NULL) {
return -ENODEV;
}
switch (opt) {
case NETOPT_MAX_PACKET_SIZE:
if (len < sizeof(int16_t)) {
return -EOVERFLOW;
}
*((uint16_t *)value) = KW2XRF_MAX_PKT_LENGTH - _MAX_MHR_OVERHEAD;
return sizeof(uint16_t);
case NETOPT_STATE:
if (len < sizeof(netopt_state_t)) {
return -EOVERFLOW;
}
*((netopt_state_t *)value) = _get_state(dev);
return sizeof(netopt_state_t);
case NETOPT_PRELOADING:
if (dev->netdev.flags & KW2XRF_OPT_PRELOADING) {
*((netopt_enable_t *)value) = NETOPT_ENABLE;
}
else {
*((netopt_enable_t *)value) = NETOPT_DISABLE;
}
return sizeof(netopt_enable_t);
case NETOPT_PROMISCUOUSMODE:
if (dev->netdev.flags & KW2XRF_OPT_PROMISCUOUS) {
*((netopt_enable_t *)value) = NETOPT_ENABLE;
}
else {
*((netopt_enable_t *)value) = NETOPT_DISABLE;
}
return sizeof(netopt_enable_t);
case NETOPT_RX_START_IRQ:
*((netopt_enable_t *)value) =
!!(dev->netdev.flags & KW2XRF_OPT_TELL_RX_START);
return sizeof(netopt_enable_t);
case NETOPT_RX_END_IRQ:
*((netopt_enable_t *)value) =
!!(dev->netdev.flags & KW2XRF_OPT_TELL_RX_END);
return sizeof(netopt_enable_t);
case NETOPT_TX_START_IRQ:
*((netopt_enable_t *)value) =
!!(dev->netdev.flags & KW2XRF_OPT_TELL_TX_START);
return sizeof(netopt_enable_t);
case NETOPT_TX_END_IRQ:
*((netopt_enable_t *)value) =
!!(dev->netdev.flags & KW2XRF_OPT_TELL_TX_END);
return sizeof(netopt_enable_t);
case NETOPT_AUTOCCA:
*((netopt_enable_t *)value) =
!!(dev->netdev.flags & KW2XRF_OPT_AUTOCCA);
return sizeof(netopt_enable_t);
case NETOPT_TX_POWER:
if (len < sizeof(int16_t)) {
return -EOVERFLOW;
}
*((uint16_t *)value) = kw2xrf_get_txpower(dev);
return sizeof(uint16_t);
case NETOPT_IS_CHANNEL_CLR:
if (kw2xrf_cca(dev)) {
*((netopt_enable_t *)value) = NETOPT_ENABLE;
}
else {
*((netopt_enable_t *)value) = NETOPT_DISABLE;
}
return sizeof(netopt_enable_t);
case NETOPT_CCA_THRESHOLD:
if (len < sizeof(uint8_t)) {
return -EOVERFLOW;
}
else {
*(int8_t *)value = kw2xrf_get_cca_threshold(dev);
}
return sizeof(int8_t);
case NETOPT_CCA_MODE:
if (len < sizeof(uint8_t)) {
return -EOVERFLOW;
}
else {
*(uint8_t *)value = kw2xrf_get_cca_mode(dev);
switch (*((int8_t *)value)) {
case NETDEV_IEEE802154_CCA_MODE_1:
case NETDEV_IEEE802154_CCA_MODE_2:
case NETDEV_IEEE802154_CCA_MODE_3:
return sizeof(uint8_t);
default:
break;
}
return -EOVERFLOW;
}
break;
case NETOPT_CHANNEL_PAGE:
default:
break;
}
return netdev_ieee802154_get((netdev_ieee802154_t *)netdev, opt, value, len);
}
