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