Hello community,
I am feeding Obstacle Distance message from my companion computer(Jetson Nano) via the Obstacle Distance message(330). My ArduPilot is receiving the obstacle message, at the specified rate(10-11Hz). Currently I am just feeding dummy values, (all 72 elements are set to 300). When I enable the proximity avoidance, and setup a mission(via QGC), the boat barely moves. However, when I disable to proximity avoidance, the boat starts moving, accelerates to cruise speed and is able to carry out the mission and complete it. The moment I enable proximity avoidance, the boat stops. Avoid margin is set to 1m, hence sending a value of 3m(300) should allow the boat to move.
Has anyone else faced this issue before? I have attached the latest log file(cloud upload), and also the c++ function, which I use to create the message and send it over to ArduPilot.
int Rover::setDistance(uint16_t (&distance)[72])
{
// OBSTACLE_DISTANCE (330)
// Format Data:
mavlink_obstacle_distance_t obstacleDistance = {0}; // set all fields to zero
obstacleDistance.time_usec = (uint32_t)(getCurrentTimeUsec());
obstacleDistance.sensor_type = 0; //was 1
memcpy(obstacleDistance.distances, distance, sizeof(distance));
obstacleDistance.increment = 5;
obstacleDistance.min_distance = 10;
obstacleDistance.max_distance = 1500;
obstacleDistance.frame = MAV_FRAME_BODY_FRD;
obstacleDistance.increment_f = 0;
obstacleDistance.angle_offset = 0;
// Encode
mavlink_message_t msg;
mavlink_msg_obstacle_distance_encode(1, 158, &msg, &obstacleDistance);
// Write in the serial
int len = serial_port.write_message(msg);
if (len <= 0)
{
printf("Failed to Send");
}
//request_GPS_message();
//mavlink_global_position_int_t msg = recv_data_gps();
// printf("Distances Published");
return (len);
}
uint64_t Rover::getCurrentTimeUsec()
{
auto now = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch());
return duration.count();
}
The function setDistance(uint16_t (&distance)[72]) is called from the main loop at 10Hz and currently all values are forced to be 300. My other parameters are as follows
Jetson connected to Ardupilot over Serial 2(Telemetery 2)
PRX1_Type = 2(mavlink)
Avoid_Enable = 7
Avoid_Margin = 1
Avoid_Behave = Stop
Backup_Spd = 0
Log File: Box