// ------------------------------------------------------------------------------------------------- void SensorMap::update(SensorModel & model, float data) { // ------------------------------------------------------------------------------------------------- //std::cout << "Update sensor" << std::endl; model.insert(data); std::vector<Vector2> passed = model.getPassed(get_cell_size()); std::vector<Vector2> blocked = model.getBlocked(get_cell_size()); for (std::vector<Vector2>::iterator el = passed.begin(); el != passed.end(); ++el) { size_t row = get_map_center_row() - el->get_y(); size_t col = get_map_center_col() + el->get_x(); if (row >= 0 && row < get_map_height() && col >= 0 && col < get_map_width()) map_array_[row][col]->update(1); } for (std::vector<Vector2>::iterator el = blocked.begin(); el != blocked.end(); ++el) { size_t row = get_map_center_row() - el->get_y(); size_t col = get_map_center_col() + el->get_x(); if (row >= 0 && row < get_map_height() && col >= 0 && col < get_map_width()) map_array_[row][col]->update(0); } }
bool GridMap::_get(const StringName &p_name, Variant &r_ret) const { String name = p_name; if (name == "theme") { r_ret = get_theme(); } else if (name == "cell_size") { r_ret = get_cell_size(); } else if (name == "cell_octant_size") { r_ret = get_octant_size(); } else if (name == "cell_center_x") { r_ret = get_center_x(); } else if (name == "cell_center_y") { r_ret = get_center_y(); } else if (name == "cell_center_z") { r_ret = get_center_z(); } else if (name == "cell_scale") { r_ret = cell_scale; } else if (name == "data") { Dictionary d; PoolVector<int> cells; cells.resize(cell_map.size() * 3); { PoolVector<int>::Write w = cells.write(); int i = 0; for (Map<IndexKey, Cell>::Element *E = cell_map.front(); E; E = E->next(), i++) { encode_uint64(E->key().key, (uint8_t *)&w[i * 3]); encode_uint32(E->get().cell, (uint8_t *)&w[i * 3 + 2]); } } d["cells"] = cells; r_ret = d; } else if (name.begins_with("areas/")) { int which = name.get_slicec('/', 1).to_int(); String what = name.get_slicec('/', 2); if (what == "bounds") r_ret = area_get_bounds(which); else if (what == "name") r_ret = area_get_name(which); else if (what == "disable_distance") r_ret = area_get_portal_disable_distance(which); else if (what == "exterior_portal") r_ret = area_is_exterior_portal(which); else return false; } else return false; return true; }
bool GridMap::_get(const StringName &p_name, Variant &r_ret) const { String name = p_name; if (name == "theme") { r_ret = get_theme(); } else if (name == "cell_size") { r_ret = get_cell_size(); } else if (name == "cell_octant_size") { r_ret = get_octant_size(); } else if (name == "cell_center_x") { r_ret = get_center_x(); } else if (name == "cell_center_y") { r_ret = get_center_y(); } else if (name == "cell_center_z") { r_ret = get_center_z(); } else if (name == "cell_scale") { r_ret = cell_scale; } else if (name == "data") { Dictionary d; PoolVector<int> cells; cells.resize(cell_map.size() * 3); { PoolVector<int>::Write w = cells.write(); int i = 0; for (Map<IndexKey, Cell>::Element *E = cell_map.front(); E; E = E->next(), i++) { encode_uint64(E->key().key, (uint8_t *)&w[i * 3]); encode_uint32(E->get().cell, (uint8_t *)&w[i * 3 + 2]); } } d["cells"] = cells; r_ret = d; } else return false; return true; }
/* * Convert and fold provided ATAGs into the provided FDT. * * REturn values: * = 0 -> pretend success * = 1 -> bad ATAG (may retry with another possible ATAG pointer) * < 0 -> error from libfdt */ int atags_to_fdt(void *atag_list, void *fdt, int total_space) { struct tag *atag = atag_list; /* In the case of 64 bits memory size, need to reserve 2 cells for * address and size for each bank */ uint32_t mem_reg_property[2 * 2 * NR_BANKS]; int memcount = 0; int ret, memsize; /* make sure we've got an aligned pointer */ if ((u32)atag_list & 0x3) return 1; /* if we get a DTB here we're done already */ if (*(u32 *)atag_list == fdt32_to_cpu(FDT_MAGIC)) return 0; /* validate the ATAG */ if (atag->hdr.tag != ATAG_CORE || (atag->hdr.size != tag_size(tag_core) && atag->hdr.size != 2)) return 1; /* let's give it all the room it could need */ ret = fdt_open_into(fdt, fdt, total_space); if (ret < 0) return ret; for_each_tag(atag, atag_list) { if (atag->hdr.tag == ATAG_CMDLINE) { /* Append the ATAGS command line to the device tree * command line. * NB: This means that if the same parameter is set in * the device tree and in the tags, the one from the * tags will be chosen. */ if (do_extend_cmdline) merge_fdt_bootargs(fdt, atag->u.cmdline.cmdline); else setprop_string(fdt, "/chosen", "bootargs", atag->u.cmdline.cmdline); } else if (atag->hdr.tag == ATAG_MEM) { if (memcount >= sizeof(mem_reg_property)/4) continue; if (!atag->u.mem.size) continue; memsize = get_cell_size(fdt); if (memsize == 2) { /* if memsize is 2, that means that * each data needs 2 cells of 32 bits, * so the data are 64 bits */ uint64_t *mem_reg_prop64 = (uint64_t *)mem_reg_property; mem_reg_prop64[memcount++] = cpu_to_fdt64(atag->u.mem.start); mem_reg_prop64[memcount++] = cpu_to_fdt64(atag->u.mem.size); } else { mem_reg_property[memcount++] = cpu_to_fdt32(atag->u.mem.start); mem_reg_property[memcount++] = cpu_to_fdt32(atag->u.mem.size); } } else if (atag->hdr.tag == ATAG_INITRD2) { uint32_t initrd_start, initrd_size; initrd_start = atag->u.initrd.start; initrd_size = atag->u.initrd.size; setprop_cell(fdt, "/chosen", "linux,initrd-start", initrd_start); setprop_cell(fdt, "/chosen", "linux,initrd-end", initrd_start + initrd_size); } } if (memcount) { setprop(fdt, "/memory", "reg", mem_reg_property, 4 * memcount * memsize); } return fdt_pack(fdt); }
int atags_to_fdt(void *atag_list, void *fdt, int total_space) { struct tag *atag = atag_list; /* In the case of 64 bits memory size, need to reserve 2 cells for * address and size for each bank */ /* IAMROOT-12A: * ------------ * 64비트 시스템의 경우 한 개 뱅크당 주소와 사이즈((2 + 2) x sizeof(int32)) */ uint32_t mem_reg_property[2 * 2 * NR_BANKS]; int memcount = 0; int ret, memsize; /* make sure we've got an aligned pointer */ if ((u32)atag_list & 0x3) return 1; /* IAMROOT-12A: * ------------ * atag/DTB 포인터에서 DTB 매직넘버를 발견하면 이미 DTB가 존재하는 것으로 파악이되어 * ATAG를 컨버전할 필요 없으므로 성공으로 리턴 */ /* if we get a DTB here we're done already */ if (*(u32 *)atag_list == fdt32_to_cpu(FDT_MAGIC)) return 0; /* IAMROOT-12A: * ------------ * 처음에 오는 태크가 ATAG_CORE가 아니거나 사이즈가 맞지않으면 실패(1)로 리턴 */ /* validate the ATAG */ if (atag->hdr.tag != ATAG_CORE || (atag->hdr.size != tag_size(tag_core) && atag->hdr.size != 2)) return 1; /* let's give it all the room it could need */ ret = fdt_open_into(fdt, fdt, total_space); if (ret < 0) return ret; /* IAMROOT-12A: * ------------ * atag_list는 atag 개채의 묶음. * for_each_tag()를 수행 시 atag에 하나의 ATAG를 가리키는 포인터가 담김 * 태그는 3개(ATAG_CMDLINE, ATAG_MEM, ATAG_INITRD2)만 디바이스트리로 컨버전 * - ATAG_CMDLINE(cmdline) ---> DTB:/chosen 노드 -> bootargs 프로퍼티 * - ATAG_MEM(u.mem.start & u.mem.size x N뱅크) ---> DTB:/memory 노드 -> reg 프로퍼티 * - ATAG_INITRD2(u.initrd.start & u.initrd.size) ---> DTB:/chosen 노드 -> linux,initrd-start * ---> DTB:/chosen 노드 -> linux,initrd-end */ for_each_tag(atag, atag_list) { if (atag->hdr.tag == ATAG_CMDLINE) { /* Append the ATAGS command line to the device tree * command line. * NB: This means that if the same parameter is set in * the device tree and in the tags, the one from the * tags will be chosen. */ if (do_extend_cmdline) merge_fdt_bootargs(fdt, atag->u.cmdline.cmdline); else setprop_string(fdt, "/chosen", "bootargs", atag->u.cmdline.cmdline); } else if (atag->hdr.tag == ATAG_MEM) { if (memcount >= sizeof(mem_reg_property)/4) continue; if (!atag->u.mem.size) continue; memsize = get_cell_size(fdt); /* IAMROOT-12A: * ------------ * memsize=2인 경우 64비트 * ATAG_MEM은 여러 개가 존재할 수 있다. */ if (memsize == 2) { /* if memsize is 2, that means that * each data needs 2 cells of 32 bits, * so the data are 64 bits */ uint64_t *mem_reg_prop64 = (uint64_t *)mem_reg_property; mem_reg_prop64[memcount++] = cpu_to_fdt64(atag->u.mem.start); mem_reg_prop64[memcount++] = cpu_to_fdt64(atag->u.mem.size); } else { mem_reg_property[memcount++] = cpu_to_fdt32(atag->u.mem.start); mem_reg_property[memcount++] = cpu_to_fdt32(atag->u.mem.size); } } else if (atag->hdr.tag == ATAG_INITRD2) { uint32_t initrd_start, initrd_size; initrd_start = atag->u.initrd.start; initrd_size = atag->u.initrd.size; setprop_cell(fdt, "/chosen", "linux,initrd-start", initrd_start); setprop_cell(fdt, "/chosen", "linux,initrd-end", initrd_start + initrd_size); } } if (memcount) { setprop(fdt, "/memory", "reg", mem_reg_property, 4 * memcount * memsize); } return fdt_pack(fdt); }
/* * Convert and fold provided ATAGs into the provided FDT. * * REturn values: * = 0 -> pretend success * = 1 -> bad ATAG (may retry with another possible ATAG pointer) * < 0 -> error from libfdt */ int atags_to_fdt(void *atag_list, void *fdt, int total_space) { struct tag *atag = atag_list; /* In the case of 64 bits memory size, need to reserve 2 cells for * address and size for each bank */ uint32_t mem_reg_property[2 * 2 * NR_BANKS]; int memcount = 0; int ret, memsize; /* make sure we've got an aligned pointer */ if ((u32)atag_list & 0x3) return 1; /* if we get a DTB here we're done already */ if (*(u32 *)atag_list == fdt32_to_cpu(FDT_MAGIC)) return 0; /* validate the ATAG */ if (atag->hdr.tag != ATAG_CORE || (atag->hdr.size != tag_size(tag_core) && atag->hdr.size != 2)) return 1; /* let's give it all the room it could need */ ret = fdt_open_into(fdt, fdt, total_space); if (ret < 0) return ret; for_each_tag(atag, atag_list) { if (atag->hdr.tag == ATAG_CMDLINE) { /* Append the ATAGS command line to the device tree * command line. * NB: This means that if the same parameter is set in * the device tree and in the tags, the one from the * tags will be chosen. */ if (do_extend_cmdline) merge_fdt_bootargs(fdt, atag->u.cmdline.cmdline); else setprop_string(fdt, "/chosen", "bootargs", atag->u.cmdline.cmdline); } else if (atag->hdr.tag == ATAG_MEM) { if (memcount >= sizeof(mem_reg_property)/4) continue; if (!atag->u.mem.size) continue; memsize = get_cell_size(fdt); if (memsize == 2) { /* if memsize is 2, that means that * each data needs 2 cells of 32 bits, * so the data are 64 bits */ uint64_t *mem_reg_prop64 = (uint64_t *)mem_reg_property; mem_reg_prop64[memcount++] = cpu_to_fdt64(atag->u.mem.start); mem_reg_prop64[memcount++] = cpu_to_fdt64(atag->u.mem.size); } else { mem_reg_property[memcount++] = cpu_to_fdt32(atag->u.mem.start); mem_reg_property[memcount++] = cpu_to_fdt32(atag->u.mem.size); } } else if (atag->hdr.tag == ATAG_INITRD2) { uint32_t initrd_start, initrd_size; initrd_start = atag->u.initrd.start; initrd_size = atag->u.initrd.size; setprop_cell(fdt, "/chosen", "linux,initrd-start", initrd_start); setprop_cell(fdt, "/chosen", "linux,initrd-end", initrd_start + initrd_size); } else if (atag->hdr.tag == ATAG_BLUETOOTH) { setprop_values(fdt, "/chosen", "linux,bt_mac", (unsigned char *)(&atag->u), (atag->hdr.size-2)*sizeof(__u32)); } else if (atag->hdr.tag == ATAG_MSM_WIFI) { #define NVS_MAX_SIZE 0x800U #define NVS_LEN_OFFSET 0x0C #define NVS_DATA_OFFSET 0x40 char append[] = "\nsd_oobonly=1\nbtc_params80=0\nbtc_params6=30\n"; __u32 len = 0; __u32 full_len = (atag->hdr.size-2)*sizeof(__u32); // check that we have enought space for get len if (full_len > NVS_LEN_OFFSET) memcpy(&len, (unsigned char *)(&atag->u) + NVS_LEN_OFFSET, sizeof(len)); // len is less than full block size if (len > (NVS_MAX_SIZE - NVS_DATA_OFFSET)) len = (NVS_MAX_SIZE - NVS_DATA_OFFSET); // len is less than atag block size if (len > full_len) len = full_len; // we have enought space for add additional params if ((len + strlen(append) + 1) <= full_len) { // block is finished by zero if (((unsigned char *)(&atag->u))[NVS_DATA_OFFSET + len] == 0) len --; //copy additional params memcpy( (unsigned char *)(&atag->u) + NVS_DATA_OFFSET + len, append, strlen(append) + 1 ); len += strlen(append); len ++; } // finaly save new wifi calibration setprop_values(fdt, "/chosen", "linux,wifi-calibration", (unsigned char *)(&atag->u) + NVS_DATA_OFFSET, len); } else if (atag->hdr.tag == ATAG_MSM_AWB_CAL) { setprop_values(fdt, "/chosen", "linux,awb_cal", (unsigned char *)(&atag->u), (atag->hdr.size-2)*sizeof(__u32)); } else if (atag->hdr.tag == ATAG_MFG_GPIO_TABLE) { setprop_values(fdt, "/chosen", "linux,gpio_table", (unsigned char *)(&atag->u), (atag->hdr.size-2)*sizeof(__u32)); } else if (atag->hdr.tag == ATAG_MSM_PARTITION) { setprop_values(fdt, "/chosen", "linux,msm_partitions", (unsigned char *)(&atag->u), (atag->hdr.size-2)*sizeof(__u32)); } else if (atag->hdr.tag == ATAG_MEMSIZE) { setprop_cell(fdt, "/chosen", "linux,memsize", atag->u.revision.rev); } else if (atag->hdr.tag == ATAG_ALS) { setprop_cell(fdt, "/chosen", "linux,als_calibration", atag->u.als_kadc.kadc); } else if (atag->hdr.tag == ATAG_ENGINEERID) { setprop_cell(fdt, "/chosen", "linux,engineerid", atag->u.revision.rev); } else if (atag->hdr.tag == ATAG_SMI) { setprop_cell(fdt, "/chosen", "linux,smi", atag->u.mem.size); } else if (atag->hdr.tag == ATAG_HWID) { setprop_cell(fdt, "/chosen", "linux,hwid", atag->u.revision.rev); } else if (atag->hdr.tag == ATAG_SKUID) { setprop_cell(fdt, "/chosen", "linux,skuid", atag->u.revision.rev); } else if (atag->hdr.tag == ATAG_HERO_PANEL_TYPE) { setprop_cell(fdt, "/chosen", "linux,panel_type", atag->u.revision.rev); } else if (atag->hdr.tag == ATAG_GS) { setprop_cell(fdt, "/chosen", "linux,gs_calibration", atag->u.revision.rev); } else if (atag->hdr.tag == ATAG_REVISION) { __u32 revision[2]; revision[0] = cpu_to_fdt32(atag->u.revision.rev); revision[1] = cpu_to_fdt32(atag->u.revision.rev); if (atag->hdr.size > 3) { revision[1] = cpu_to_fdt32(atag->u.revision.rev2); } setprop_values(fdt, "/chosen", "linux,revision", revision, sizeof(revision)); } else if (atag->hdr.tag == ATAG_PS) { __u32 ps_settings[2]; ps_settings[0] = cpu_to_fdt32(atag->u.serialnr.low); ps_settings[1] = cpu_to_fdt32(atag->u.serialnr.high); setprop_values(fdt, "/chosen", "linux,ps_calibration", ps_settings, sizeof(ps_settings)); } else if (atag->hdr.tag == ATAG_PS_TYPE) { setprop_cell(fdt, "/chosen", "linux,ps_type", atag->u.revision.rev); } } if (memcount) { setprop(fdt, "/memory", "reg", mem_reg_property, 4 * memcount * memsize); } return fdt_pack(fdt); }