|
|
|
@ -97,7 +97,7 @@ float SerialProximitySensor::measure_distance_at_angle_bf(const Location &locati
@@ -97,7 +97,7 @@ float SerialProximitySensor::measure_distance_at_angle_bf(const Location &locati
|
|
|
|
|
for (int8_t x=-num_post_offset; x<num_post_offset; x++) { |
|
|
|
|
for (int8_t y=-num_post_offset; y<num_post_offset; y++) { |
|
|
|
|
Location post_location = post_origin; |
|
|
|
|
post_location.offset(x*10+3, y*10+2); |
|
|
|
|
post_location.offset_double(x*10+3, y*10+2); |
|
|
|
|
if (postfile != nullptr) { |
|
|
|
|
::fprintf(postfile, "map circle %f %f %f blue\n", post_location.lat*1e-7, post_location.lng*1e-7, radius_cm/100.0); |
|
|
|
|
} |
|
|
|
@ -111,8 +111,8 @@ float SerialProximitySensor::measure_distance_at_angle_bf(const Location &locati
@@ -111,8 +111,8 @@ float SerialProximitySensor::measure_distance_at_angle_bf(const Location &locati
|
|
|
|
|
float dist_cm = (intersection_point_cm-vehicle_pos_cm).length(); |
|
|
|
|
if (intersectionsfile != nullptr) { |
|
|
|
|
Location intersection_point = location; |
|
|
|
|
intersection_point.offset(intersection_point_cm.x/100.0f, |
|
|
|
|
intersection_point_cm.y/100.0f); |
|
|
|
|
intersection_point.offset_double(intersection_point_cm.x/100.0, |
|
|
|
|
intersection_point_cm.y/100.0); |
|
|
|
|
::fprintf(intersectionsfile, |
|
|
|
|
"map icon %f %f barrell\n", |
|
|
|
|
intersection_point.lat*1e-7, |
|
|
|
|