/**
  * This function provides NAS-QoS priority_group CPS API read function
  * @Param      Standard CPS API params
  * @Return   Standard Error Code
  */
cps_api_return_code_t nas_qos_cps_api_priority_group_read (void * context,
                                            cps_api_get_params_t * param,
                                            size_t ix)
{
    cps_api_object_t obj = cps_api_object_list_get(param->filters, ix);
    cps_api_object_attr_t port_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_PORT_ID);
    cps_api_object_attr_t local_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_LOCAL_ID);

    uint_t switch_id = 0;

    if (port_id_attr == NULL) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Port Id must be specified\n");
        return NAS_QOS_E_MISSING_KEY;
    }

    uint_t port_id = cps_api_object_attr_data_u32(port_id_attr);

    bool local_id_specified = false;
    uint8_t local_id;
    if (local_id_attr) {
        local_id = *(uint8_t *)cps_api_object_attr_data_bin(local_id_attr);
        local_id_specified = true;
    }

    EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Read switch id %u, port_id id %u\n",
                    switch_id, port_id);

    std_mutex_simple_lock_guard p_m(&priority_group_mutex);

    // If the port priority group is not initialized yet, initialize it in NAS
    nas_qos_port_priority_group_init(port_id);

    return nas_qos_cps_get_priority_group_info(param, switch_id, port_id,
                                            local_id_specified, local_id);
}
/**
  * This function provides NAS-QoS priority_group CPS API rollback function
  * @Param      Standard CPS API params
  * @Return   Standard Error Code
  */
cps_api_return_code_t nas_qos_cps_api_priority_group_rollback(void * context,
                                            cps_api_transaction_params_t * param,
                                            size_t ix)
{
    cps_api_object_t obj = cps_api_object_list_get(param->prev,ix);
    cps_api_operation_types_t op = cps_api_object_type_operation(cps_api_object_key(obj));

    std_mutex_simple_lock_guard p_m(&priority_group_mutex);

    if (op == cps_api_oper_SET) {
        nas_qos_cps_api_priority_group_set(obj, NULL);
    }

    return cps_api_ret_code_OK;
}
示例#3
0
文件: MCL.cpp 项目: Flavkupe/nao-man
/**
 * Initializes the sampel sets so that the first update works appropriately
 */
MCL::MCL()
{
    // Initialize particles to be randomly spread about the field...
    srand(time(NULL));
    for (int m = 0; m < M; ++m) {
        //Particle p_m;
        // X bounded by width of the field
        // Y bounded by height of the field
        // H between +-pi
        PoseEst x_m(float(rand() % int(FIELD_WIDTH)),
                    float(rand() % int(FIELD_HEIGHT)),
                    float(((rand() % FULL_CIRC) - HALF_CIRC))*DEG_TO_RAD);
        Particle p_m(x_m, 1.0f);
        // p_m.pose = x_m;
        // p_m.weight = 1;
        X_t.push_back(p_m);
    }

    updateEstimates();
}
/**
  * This function provides NAS-QoS priority_group CPS API write function
  * @Param      Standard CPS API params
  * @Return   Standard Error Code
  */
cps_api_return_code_t nas_qos_cps_api_priority_group_write(void * context,
                                            cps_api_transaction_params_t * param,
                                            size_t ix)
{
    cps_api_object_t obj = cps_api_object_list_get(param->change_list,ix);
    cps_api_operation_types_t op = cps_api_object_type_operation(cps_api_object_key(obj));

    std_mutex_simple_lock_guard p_m(&priority_group_mutex);

    switch (op) {
    case cps_api_oper_CREATE:
    case cps_api_oper_DELETE:
        return NAS_QOS_E_FAIL; //not supported

    case cps_api_oper_SET:
        return nas_qos_cps_api_priority_group_set(obj, param->prev);

    default:
        return NAS_QOS_E_UNSUPPORTED;
    }
}
template<typename Scalar, int Dim, int Options> void transform_products()
{
  typedef Matrix<Scalar,Dim+1,Dim+1> Mat;
  typedef Transform<Scalar,Dim,Projective,Options> Proj;
  typedef Transform<Scalar,Dim,Affine,Options> Aff;
  typedef Transform<Scalar,Dim,AffineCompact,Options> AffC;

  Proj p; p.matrix().setRandom();
  Aff a; a.linear().setRandom(); a.translation().setRandom();
  AffC ac = a;

  Mat p_m(p.matrix()), a_m(a.matrix());

  VERIFY_IS_APPROX((p*p).matrix(), p_m*p_m);
  VERIFY_IS_APPROX((a*a).matrix(), a_m*a_m);
  VERIFY_IS_APPROX((p*a).matrix(), p_m*a_m);
  VERIFY_IS_APPROX((a*p).matrix(), a_m*p_m);
  VERIFY_IS_APPROX((ac*a).matrix(), a_m*a_m);
  VERIFY_IS_APPROX((a*ac).matrix(), a_m*a_m);
  VERIFY_IS_APPROX((p*ac).matrix(), p_m*a_m);
  VERIFY_IS_APPROX((ac*p).matrix(), a_m*p_m);
}
/**
  * This function provides NAS-QoS priority_group stats CPS API clear function
  * To clear the priority_group stats, set relevant counters to zero
  * @Param    Standard CPS API params
  * @Return   Standard Error Code
  */
cps_api_return_code_t nas_qos_cps_api_priority_group_stat_clear (void * context,
                                            cps_api_transaction_params_t * param,
                                            size_t ix)
{
    cps_api_object_t obj = cps_api_object_list_get(param->change_list,ix);
    cps_api_operation_types_t op = cps_api_object_type_operation(cps_api_object_key(obj));

    if (op != cps_api_oper_SET)
        return NAS_QOS_E_UNSUPPORTED;

    cps_api_object_attr_t port_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID);
    cps_api_object_attr_t local_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID);

    if (port_id_attr == NULL ||
        local_id_attr == NULL) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
                "Incomplete key: port-id, priority_group local id must be specified\n");
        return NAS_QOS_E_MISSING_KEY;
    }
    uint32_t switch_id = 0;
    nas_qos_priority_group_key_t key;
    key.port_id = cps_api_object_attr_data_u32(port_id_attr);
    key.local_id = *(uint8_t *)cps_api_object_attr_data_bin(local_id_attr);


    EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
            "Read switch id %u, port_id %u priority_group local_id %u stat\n",
            switch_id, key.port_id,  key.local_id);

    std_mutex_simple_lock_guard p_m(&priority_group_mutex);

    // If the port is not be initialized yet, initialize it in NAS
    nas_qos_port_priority_group_init(key.port_id);

    nas_qos_switch *switch_p = nas_qos_get_switch(switch_id);
    if (switch_p == NULL) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "switch_id %u not found", switch_id);
        return NAS_QOS_E_FAIL;
    }

    nas_qos_priority_group * priority_group_p = switch_p->get_priority_group(key);
    if (priority_group_p == NULL) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Priority Group not found");
        return NAS_QOS_E_FAIL;
    }

    std::vector<BASE_QOS_PRIORITY_GROUP_STAT_t> counter_ids;

    cps_api_object_it_t it;
    cps_api_object_it_begin(obj,&it);
    for ( ; cps_api_object_it_valid(&it) ; cps_api_object_it_next(&it) ) {
        cps_api_attr_id_t id = cps_api_object_attr_id(it.attr);
        switch (id) {
        case BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID:
        case BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID:
            break;

        case BASE_QOS_PRIORITY_GROUP_STAT_PACKETS:
        case BASE_QOS_PRIORITY_GROUP_STAT_BYTES:
        case BASE_QOS_PRIORITY_GROUP_STAT_WATERMARK_BYTES:
            counter_ids.push_back((BASE_QOS_PRIORITY_GROUP_STAT_t)id);
            break;

        case BASE_QOS_PRIORITY_GROUP_STAT_CURRENT_OCCUPANCY_BYTES:
            // READ-only
            break;

        default:
            EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
                    "Unknown priority_group STAT flag: %u, ignored", id);
            break;
        }
    }

    if (ndi_qos_clear_priority_group_stats(priority_group_p->get_ndi_port_id(),
                                priority_group_p->ndi_obj_id(),
                                &counter_ids[0],
                                counter_ids.size()) != STD_ERR_OK) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Priority Group stats clear failed");
        return NAS_QOS_E_FAIL;
    }


    return  cps_api_ret_code_OK;

}
/**
  * This function provides NAS-QoS priority_group stats CPS API read function
  * @Param    Standard CPS API params
  * @Return   Standard Error Code
  */
cps_api_return_code_t nas_qos_cps_api_priority_group_stat_read (void * context,
                                            cps_api_get_params_t * param,
                                            size_t ix)
{

    cps_api_object_t obj = cps_api_object_list_get(param->filters, ix);
    cps_api_object_attr_t port_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID);
    cps_api_object_attr_t local_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID);

    if (port_id_attr == NULL ||
        local_id_attr == NULL ) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
                "Incomplete key: port-id, priority_group local id must be specified\n");
        return NAS_QOS_E_MISSING_KEY;
    }
    uint32_t switch_id = 0;
    nas_qos_priority_group_key_t key;
    key.port_id = cps_api_object_attr_data_u32(port_id_attr);
    key.local_id = *(uint8_t *)cps_api_object_attr_data_bin(local_id_attr);


    EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
            "Read switch id %u, port_id %u priority_group local id %u stat\n",
            switch_id, key.port_id, key.local_id);

    std_mutex_simple_lock_guard p_m(&priority_group_mutex);

    // If the port is not be initialized yet, initialize it in NAS
    nas_qos_port_priority_group_init(key.port_id);

    nas_qos_switch *switch_p = nas_qos_get_switch(switch_id);
    if (switch_p == NULL) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "switch_id %u not found", switch_id);
        return NAS_QOS_E_FAIL;
    }

    nas_qos_priority_group * priority_group_p = switch_p->get_priority_group(key);
    if (priority_group_p == NULL) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Priority Group not found");
        return NAS_QOS_E_FAIL;
    }

    nas_qos_priority_group_stat_counter_t stats = {0};
    std::vector<BASE_QOS_PRIORITY_GROUP_STAT_t> counter_ids;

    cps_api_object_it_t it;
    cps_api_object_it_begin(obj,&it);
    for ( ; cps_api_object_it_valid(&it) ; cps_api_object_it_next(&it) ) {
        cps_api_attr_id_t id = cps_api_object_attr_id(it.attr);
        switch (id) {
        case BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID:
        case BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID:
            break;

        case BASE_QOS_PRIORITY_GROUP_STAT_PACKETS:
        case BASE_QOS_PRIORITY_GROUP_STAT_BYTES:
        case BASE_QOS_PRIORITY_GROUP_STAT_CURRENT_OCCUPANCY_BYTES:
        case BASE_QOS_PRIORITY_GROUP_STAT_WATERMARK_BYTES:
            counter_ids.push_back((BASE_QOS_PRIORITY_GROUP_STAT_t)id);
            break;

        default:
            EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Unknown priority_group STAT flag: %u, ignored", id);
            break;
        }
    }

    if (ndi_qos_get_priority_group_stats(priority_group_p->get_ndi_port_id(),
                                priority_group_p->ndi_obj_id(),
                                &counter_ids[0],
                                counter_ids.size(),
                                &stats) != STD_ERR_OK) {
        EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Priority Group stats get failed");
        return NAS_QOS_E_FAIL;
    }

    // return stats objects to cps-app
    cps_api_object_t ret_obj = cps_api_object_list_create_obj_and_append(param->list);
    if (ret_obj == NULL) {
        return cps_api_ret_code_ERR;
    }

    cps_api_key_from_attr_with_qual(cps_api_object_key(ret_obj),
            BASE_QOS_PRIORITY_GROUP_STAT_OBJ,
            cps_api_qualifier_TARGET);
    cps_api_set_key_data(ret_obj, BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID,
            cps_api_object_ATTR_T_U32,
            &(key.port_id), sizeof(uint32_t));
    cps_api_set_key_data(ret_obj, BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID,
            cps_api_object_ATTR_T_BIN,
            &(key.local_id), sizeof(uint8_t));

    uint64_t val64;
    for (uint_t i=0; i< counter_ids.size(); i++) {
        BASE_QOS_PRIORITY_GROUP_STAT_t id = counter_ids[i];
        val64 = get_stats_by_type(&stats, id);
        cps_api_object_attr_add_u64(ret_obj, id, val64);
    }

    return  cps_api_ret_code_OK;

}
/**
  * This function provides NAS-QoS SCHEDULER_GROUP CPS API read function
  * @Param      Standard CPS API params
  * @Return   Standard Error Code
  */
cps_api_return_code_t nas_qos_cps_api_scheduler_group_read (void * context,
                                            cps_api_get_params_t * param,
                                            size_t ix)
{
    cps_api_object_t obj = cps_api_object_list_get(param->filters, ix);
    cps_api_object_attr_t sg_attr = cps_api_get_key_data(obj, BASE_QOS_SCHEDULER_GROUP_ID);
    uint32_t port_id = 0, level;
    bool have_port = false, have_level = false;
    if (!sg_attr) {
        cps_api_object_it_t it;
        cps_api_object_it_begin(obj, &it);
        for ( ; cps_api_object_it_valid(&it); cps_api_object_it_next(&it)) {
            cps_api_attr_id_t id = cps_api_object_attr_id(it.attr);
            if (id == BASE_QOS_SCHEDULER_GROUP_PORT_ID) {
                port_id = cps_api_object_attr_data_u32(it.attr);
                have_port = true;
            } else if (id == BASE_QOS_SCHEDULER_GROUP_LEVEL) {
                level = cps_api_object_attr_data_u32(it.attr);
                have_level = true;
            }
        }
    }

    if (!sg_attr && !have_port) {
        EV_LOG_TRACE(ev_log_t_QOS, ev_log_s_MINOR, "NAS-QOS",
                     "Invalid input attributes for reading");
        return NAS_QOS_E_MISSING_KEY;
    }

    uint_t switch_id = 0;
    nas_obj_id_t scheduler_group_id = (sg_attr? cps_api_object_attr_data_u64(sg_attr): 0);

    EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Read switch id %u, scheduler_group id 0x%016lX\n",
                    switch_id, scheduler_group_id);

    std_mutex_simple_lock_guard p_m(&scheduler_group_mutex);

    if (have_port) {
        nas_qos_port_scheduler_group_init(port_id);
    }
    nas_qos_switch * p_switch = nas_qos_get_switch(switch_id);
    if (p_switch == NULL)
        return NAS_QOS_E_FAIL;

    std::vector<nas_qos_scheduler_group *> sg_list;
    if (sg_attr) {
        nas_qos_scheduler_group *scheduler_group = p_switch->get_scheduler_group(scheduler_group_id);
        if (scheduler_group == NULL) {
            EV_LOG_TRACE(ev_log_t_QOS, ev_log_s_MINOR, "NAS-QOS",
                         "Could not find scheduler group with ID %llu", scheduler_group_id);
            return NAS_QOS_E_FAIL;
        }
        sg_list.push_back(scheduler_group);
    } else {
        int match_level = have_level ? (int)level : -1;
        p_switch->get_port_scheduler_groups(port_id, match_level, sg_list);
    }

    /* fill in data */
    cps_api_object_t ret_obj;

    for (auto scheduler_group: sg_list) {
        ret_obj = cps_api_object_list_create_obj_and_append (param->list);
        if (ret_obj == NULL) {
            EV_LOG_TRACE(ev_log_t_QOS, ev_log_s_MINOR, "NAS-QOS",
                         "Failed to create cps object");
            return cps_api_ret_code_ERR;
        }

        cps_api_key_from_attr_with_qual(cps_api_object_key(ret_obj),BASE_QOS_SCHEDULER_GROUP_OBJ,
                cps_api_qualifier_TARGET);
        cps_api_set_key_data(ret_obj, BASE_QOS_SCHEDULER_GROUP_SWITCH_ID,
                cps_api_object_ATTR_T_U32,
                &switch_id, sizeof(uint32_t));
        scheduler_group_id = scheduler_group->get_scheduler_group_id();
        cps_api_set_key_data(ret_obj, BASE_QOS_SCHEDULER_GROUP_ID,
                cps_api_object_ATTR_T_U64,
                &scheduler_group_id, sizeof(uint64_t));

        cps_api_object_attr_add_u32(ret_obj,
                                    BASE_QOS_SCHEDULER_GROUP_CHILD_COUNT,
                                    scheduler_group->get_child_count());

        for (uint32_t idx = 0; idx < scheduler_group->get_child_count(); idx++) {
            cps_api_object_attr_add_u64(ret_obj,
                                        BASE_QOS_SCHEDULER_GROUP_CHILD_LIST,
                                        scheduler_group->get_child_id(idx));
        }

        cps_api_object_attr_add_u32(ret_obj, BASE_QOS_SCHEDULER_GROUP_PORT_ID,
                                    scheduler_group->get_port_id());
        cps_api_object_attr_add_u32(ret_obj, BASE_QOS_SCHEDULER_GROUP_LEVEL,
                                    scheduler_group->get_level());
        cps_api_object_attr_add_u64(ret_obj, BASE_QOS_SCHEDULER_GROUP_SCHEDULER_PROFILE_ID,
                                    scheduler_group->get_scheduler_profile_id());
    }

    return cps_api_ret_code_OK;
}