Question about OBSTACLE_DISTANCE message

imindude · March 5, 2024, 4:31am

Hi,
I’m trying to use the “Collision Avoidance” feature in use of companion computer. The system is following this,

PX4 1.14.0
MAVSDK 2.2.0
WSL / gz_x500
QGC 4.3.0

This is my message injection code

void CollisionAvoidance::run()
{
    int64_t now_ms = Utils::TimeSinceEpochMs();
    int64_t epo_ms;

    // for stabilization delay
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    std::random_device                    rd;
    std::mt19937                          rg(rd());
    MSG::ObstacleDistance                 obstacle_distance;
    std::uniform_real_distribution<float> dist(0.f, 2.f);

    for (; not _thread->interruption_requested();)
    {
        epo_ms = now_ms;
        std::this_thread::sleep_for(std::chrono::milliseconds(EVERY_RUN_MS - (now_ms - epo_ms)));
        now_ms = Utils::TimeSinceEpochMs();

        for (int n = 0; n < OBSTACLE_DISTANCE_SECTION; n++)
        {
            obstacle_distance.distance_m_[n] = 25.f + dist(rg);
        }
        obstacle_distance.inc_deg_    = 5.f;
        obstacle_distance.min_dist_m_ = 1.f;
        obstacle_distance.max_dist_m_ = 100.f;

        ObstacleDistanceUpdated(obstacle_distance);
    }
}

bool ObstacleDistanceReceived(ObstacleDistance& obstacle_distance)
{
    /// 10Hz maybe
    uint16_t distances_cm[OBSTACLE_DISTANCE_SECTION];
    uint16_t min_distance_cm = static_cast<uint16_t>(obstacle_distance.min_dist_m_ * 100.f);
    uint16_t max_distance_cm = static_cast<uint16_t>(obstacle_distance.max_dist_m_ * 100.f);

    for (int n = 0; n < OBSTACLE_DISTANCE_SECTION; n++)
        distances_cm[n] = static_cast<uint16_t>(obstacle_distance.distance_m_[n] * 100.f);

    auto result = _passthru->queue_message([&](MavlinkAddress address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_obstacle_distance_pack_chan(
            address.system_id,
            address.component_id,
            channel,
            &message,
            Utils::TimeSinceEpochUs(),
            MAV_DISTANCE_SENSOR_LASER,
            distances_cm,
            0,
            min_distance_cm,
            max_distance_cm,
            obstacle_distance.inc_deg_,
            0.f,
            MAV_FRAME_BODY_FRD
        );
        return message;
    });

    return result == mavsdk::MavlinkPassthrough::Result::Success;
}

Here is my QGC.

In “Arming Check Report:”

Avoidance system not ready

My setup of “Safety” are,

Collision Prevention : “Enabled”
Obstacle Avoidance : “Enabled”
Minimum Distance : 10

Here is MAVLink Inspector

What is my missing?