How to perform Obstacle Avoidance simulation with multiple waypoints using UAV Package Delivery and QGroundControl?

Hi everyone,

I am currently struggling to modify the UAV package delivery example project, such as the quadcopter would be able to perform a flight consisting in multiple waypoints while avoiding obstacles. I have tried for many hours to somehow combine the Full guidance Logic with the 3D Obstacle Avoidance guidance logic, but without success. I am currently trying to combine these two:

First, I tried taking the function "ComputeLPWaypoints" from normal Obstacle avoidance guidance and putting it in the full guidance to get GCSCommands, but it didn't give the bus signal required. Also, I have tried taking the Guidance Stateflow and put it in the OA guidance logic, but same result, nothing worked. I also have basic knowledge of Stateflow, so I could use that if it's easier/necessary. However, I don't know where to start.

The intention is to be able to fly a multiple waypoints mission (from QGroundControl) while avoiding obstacles. It all works just fine until I want to add more waypoints to the mission.

This is my last try so far (and the error that I got with it..):

Any help is very much appreciated.

Many thanks in advance.

Tudor

[Tudor-Stefan Tonca]

Update: I tried working in the obstacl avoidance guidance logic (non 3D) for simplicity, until I have a solution that works. I figured out that this might be a good guidance model for the purpose discussed:

However, I still have an error. After using the bus creator block to create the bus signal, it still didn't run. I assume the function computeLPWaypoints doesn't provide the To and From waypoints required. How do I modify the obstacle avoidance algorithm such as it outputs the required parameters?

This is the code within the function:

function y = computeLPWaypoints(mission, Steer,UAVState)
%#codegen

Wp1 = [mission(1).position' mission(1).params(4)];
Wp2x = mission(2).position(1);
Wp2y = mission(2).position(2);

if(~isnan(Steer))
    
    LAP_local = 6*[cos(Steer), sin(Steer)];
    LAP = [ LAP_local(1)+ UAVState.x , LAP_local(2) + UAVState.y , -11 , Steer];   
    if(isnan(LAP(4)))
        LAP(4) = 0;
    end
   
    y = [ Wp1(1,:) ; LAP ; Wp2x , Wp2y , 0 , Steer ];    
    DTD = sqrt( (LAP(1) - Wp2x)^2 + (LAP(2)-Wp2y)^2);  
    
else
    y = [ Wp1(1,:) ; UAVState.x , UAVState.y , UAVState.z-2 , 0 ; Wp2x , Wp2y , 0 , 0 ];
    DTD = sqrt( (UAVState.x - Wp2x)^2 + (UAVState.y-Wp2y)^2);
end

if(DTD<5)
    y = [ Wp1(1,:) ; Wp2x , Wp2y, -11 , 0 ; Wp2x , Wp2y , 0 , 0 ];
end


end

Answer 1

[Jianxin Sun]

A similar question is answered in https://www.mathworks.com/matlabcentral/answers/1905201-uavpathmanagerbus-issue-in-the-3d-obstacle-avoidance-uav-package-delivery#answer_1162586?s_tid=prof_contriblnk