%%Height Estimation Using Cortex Data
%simulate how a flying aircraft would react to flying over a staircase
%using the functions we created for the heightExtimation.m file
%variables to be used for function calls and calculations
begin_good_data = 45; % number of scans that return -1 at the beginning of each scan file
numAvg = 45; % number of previous data points to average for hexakopter height
damping = .05; % how damped you want the system to be
stairHeight = 200; %height of a typical stair in mm. (note value should be approximately 8 inches)
up = 1; %if value is one then it will simulate going up a staircase, else it will simulate going down
figure(2) %figure for visualization of height
clf
hold on;
%simple array that will serve as a returned value for a stair case
stairData = zeros(1,150);
%any value other than one simulates going down a staircase instead of up
if(up ~= 1)
stairHeight = -1*stairHeight;
end
%simulate a stair case by adding or subtracting the value of a stairHeight after so many scans
for k=1:length(stairData)
stairData(k) = 0;
if k>20
stairData(k) = stairData(k)+stairHeight;
end
if k>40
stairData(k) = stairData(k)+stairHeight;
end
if k>60
stairData(k) = stairData(k)+stairHeight;
end
if k>80
stairData(k) = stairData(k)+stairHeight;
end
if k>100
stairData(k) = stairData(k)+stairHeight;
end
end
for i = 1:length(stairData)
%get the height above the current stair case
currentHeight = stairData(i);
%set your flying height to 1meter above the staircase
setHeight = 1000+(currentHeight);
%average the last "numAvg" scans to determine how height the aircraft
%is flying
hexHeight = getHeightAverage(setHeight, i-1 , numAvg);
%change the altitude of the aircraft by adding or subtracting height and multiplying it by your dampening variable
if hexHeightsetHeight;
hexHeight = hexHeight-damping*abs((setHeight-hexHeight));
end
plot(i,currentHeight,'*b')
plot(i, hexHeight, 'r^')
plot(i, setHeight, 'k.')
pause(.01)
end
hold off;
xlabel('distance traveled');
ylabel('height');
legend('ground', 'hexicopter height','desired location');
%axis([0 140 0 3000]);