AP_Proximity: Add support for OBSTACLE_DISTANCE mavlink message #9418
Conversation
| _distance_upward = packet.current_distance / 100.0f; | ||
| _last_upward_update_ms = AP_HAL::millis(); | ||
| // get user configured yaw correction from front end | ||
| float yaw_correction = constrain_float(frontend.get_yaw_correction(state.instance), -360.0f, +360.0f); |
|
Just FYI, eskaflon is from elab.co.jp and I've been working with him on this. I've tested this on a real vehicle and it works. We've attached an RPLidarA2 to a TX2 on an AION Robotics rover and used ROS/mavros to send the lidar data to AP. That lidar data has been consumed by the Proximity library and resent down to the ground station so it appears like below in the MP "radar" view. |
|
@OXINARF, great, thanks! it can wait so np. txs. |
eskaflon
force-pushed
the
obstacle-distance2
branch
from
3b0b6ab
to
f7b1fda
Compare
| uint8_t sector = packet.orientation; | ||
| _angle[sector] = sector * 45; | ||
| _distance[sector] = packet.current_distance / 100.0f; | ||
| if (msg->msgid == MAVLINK_MSG_ID_DISTANCE_SENSOR) { |
There was a problem hiding this comment.
Nitpicking: a switch would be nicer.
| float increment = packet.increment; | ||
| if (packet.increment == 0) { | ||
| // assume an rplidar with 64 sectors | ||
| increment = 360.0f / 64.0f; |
There was a problem hiding this comment.
In my opinion we should reject a message without this information and not make assumptions.
There was a problem hiding this comment.
Txs for this review. OK, message will be rejected if the increment is zero.
| for (uint8_t j = 0; j < total_distances; j++) { | ||
| if (!used[j]) { | ||
| const float packet_distance_m = packet.distances[j] / 100.0f; | ||
| const float mid_angle = wrap_360(increment * dir_correction * (0.5f + j) - increment_half + yaw_correction); |
There was a problem hiding this comment.
I don't understand this math. Before the yaw and direction corrections, it was increment * (0.5f + j) - increment_half which is equal to increment * j:
increment * (0.5f + j) - increment_half =
increment * 0.5f + increment * j - increment_half =
increment_half + increment * j - increment_half =
increment * j
I think what was wanted was increment * j + increment_half. But this new version is more confusing:
increment * dir_correction * (0.5f + j) - increment_half + yaw_correction =
increment * dir_correction * 0.5f + increment * dir_correction * j - increment_half + yaw_correction =
dir_correction * increment_half + increment * dir_correction * j - increment_half + yaw_correction =
(dir_correction - 1) * increment_half + increment * dir_correction * j + yaw_correction
I think the old code bug just carried to the new code, but I might be wrong.
There was a problem hiding this comment.
@OXINARF, I think you're right that the old formula was overly complicated. I've simplified it to, "const float mid_angle = wrap_360(j * increment * dir_correction + yaw_correction);". In each element of the OBSTACLE_DISTANCE the middle-angle is just the increment * j. So for example if increment is 5, the 0th element is pointing straight ahead or 0deg (-2.5deg ~ 2.5deg). the 1st element's middle is at 5deg (2.5deg ~ 7.5deg), etc.
| if (!used[j]) { | ||
| const float packet_distance_m = packet.distances[j] / 100.0f; | ||
| const float mid_angle = wrap_360(increment * dir_correction * (0.5f + j) - increment_half + yaw_correction); | ||
| float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - mid_angle)); |
There was a problem hiding this comment.
Although I understand this, I think it's wrong to do it this way. Example:
- increment: 11
- distances: [900, 800, 700, 600, 100, ...]
- our driver has 8 sectores with a width of 45 degrees
- last sector from the MAVLink message is of 44-55 degrees range
- so, I would say that, in theory, its distance should for go for the driver sectors 1 (0-45) and 2 (45-90)
- but, the current code won't do count it for sector 1, which would be an issue since the sensor has detected an object at 1 meter that could at 44 degrees
In my opinion, if the MAVLink sector is within our driver sector, then it should be accounted for, not just check the MAVLink sector center.
There was a problem hiding this comment.
txs again for this review. I definitely hear what you're saying but I'm not sure it's worth supporting a single sector from the obstacle-distance message falling into two sectors within the AP_Proximity library. The reason is that in practice the obstacle-distance sectors are quite narrow and for object avoidance, we use AP_Proximity's _angle[i] and _distance[i] arrays to build a mini fence around the vehicle. the _angle[i] should never fall into two AP_Proximity sectors.
The change you're suggesting would only affect the radar view in the ground station (which doesn't use the _angle[i] array) and the difference would be quite subtle.
| used[j] = true; | ||
| } | ||
| } | ||
| updated = true; |
There was a problem hiding this comment.
This is always setting updated to true, making it useless. I'm not sure if there is a bug here or if it's simply not needed.
There was a problem hiding this comment.
yes, definitely a bug. Moved up a few lines to the moment we update the _distance[i] and _angle[i]
|
Re the assumption that the range is always 360deg - I think if the range is more than 360 there are two possibilities:
|
|
I'm closing this PR in favour of this new PR which builds upon this one. |
This PR addresses issue #9269 as it adds support for the OBSTACLE_DISTANCE message, it repopulates the 72 sectors covered by this message into 8 like the DISTANCE_SENSOR, the message is then handled the same way that the DISTANCE_SENSOR is.