Problems with modifying a driving strategy

Hey there! I have a big program that needs to calculate a certain power at a waypoint. It's really really big (and of that really big part I am sure it works) but there is just this little bit that needs to be added. To give you a bit of context:

this is part of a driving strategy for a car. It calculates the amount of work a car has to deliver when driving around the track and then tries to minimise that. That is however not the right way: it is a car that runs on hydrogen and the amount of hydrogen spent needs to be minimised. And because the efficiency of the fuel cell is not constant, the driving strategy needs a bit tweaking.

The effiency of a fuel cell can be found using a PQ diagram. It shows how much hydrogen (Q) is used when the fuel cell needs to deliver a certain power P. The formula for converting power P into hydrogen usage Q is this:

Q = 79.987*P+42.942;

That needs to be put in this part:

function [v Wtot] = startcalculation2()
%This function is executed when "calculate" button is pressed

      %   v       a nx1 matrix with all the optimal velocities [m/s]
      %   Wtot    the work at these velocities [J]

%%Takes all input data from the root sea
trackData   = getappdata(0,'trackSea');
airData     = getappdata(0,'airSea');
settingData = getappdata(0,'settingSea');
calcData    = getappdata(0,'calcSea');

%%Set data as input for the calculation:  
s           =   trackData.vectors;      %[-]    ix2
t           =   trackData.time_limit;   %[s]    1x1

%%Precalculations:

%Air friction precalculations:
    airData.windvec=airData.wind_dir/norm(airData.wind_dir)*airData.wind_velocity; %[m/s]
    setappdata(0,'airSea', airData);

%%Create the figure for the converge plot:

      calcData.converge = 0;
      setappdata(0,'calcSea',calcData);
      %set converge count to 0:
      if settingData.converge_plot==1; 
          figure(101);
          clf;
          set(101,'Name','Converge plot work optimisation'); 
          plotconverge = axes('Parent', 101 );
          xlabel('{\itIteration count}')
          ylabel('{\itWork} (Nm)')
      else
      end

%%OPTIMISATION:

%special for lsqnonlin, lower bound:
 %lb = ones(7,1);

   %for j=1:trackData.number_points-2
   %    lb(j)=norm(trackData.vectors(j,:))/trackData.time_limitl
   %end

%Starting point for the algorithm:
    v0=ones([trackData.number_points-2],1)*trackData.average_speed;
    %v0=ones([trackData.number_points-2],1)*1;

%options for the algorithm:
    options = optimset('algorithm','trust-region-reflective','LargeScale','off');
    options.TolFun  = 1e0;
    options.TolX    = 1e0;
    options.MaxFunEvals = 1e6;
    options.MaxIter     = 1e6;

% Simple optimisation:
    [x fval] = fminsearch(@calcfunction2,v0,options);
    v=x;

% Simple optimisation for squares:
   %[x fval] = lsqnonlin(@xcalcfunctionINC,v0,[],[],options)

% The old constraint optimisation:
   %[x, fval] = fmincon(@xcalcfunctionINC,v0,[],[],[],[],[],[],...
   %                    @xnonlinconINC,...
   %                     options);
   %v=x;

%Calculate the last velocity variable and display result:
    n = size(s,1);

      trest=0;
      for j=1:(n-1)
          trest=trest + norm(s(j,:))./v(j,:);
      end

      v(n,:)= ( norm(s(n,:)) )/( t - trest );

%total work:
    Wtot = fval;

%check if time constraint is made:
    ii=trackData.number_points-1;

      s=zeros(ii,1);
      for j=1:ii
          s(j)=norm(trackData.vectors(j,:));
      end

      t=sum(s./v)

% Plot the final velocity if its a 2 variable track:    
if settingData.iteration_plot==1;
    fig_iter=21;
    q = figure(fig_iter);
    aa = isempty(findobj('name','2 segment track'));

      if aa == 1;
      else
          plot(v(1),v(2),'o','LineWidth',1,...
                      'MarkerEdgeColor','k',...
                      'MarkerFaceColor','g',...
                      'MarkerSize',10);

          hold off       
      end
  else
  end

end

I tried adding the following:

W/t.= P Q = 79.987*P+42.942; Ptot=sum(P)

And then editing the minimalisation part. That is currently this:

[x fval] = fminsearch(@calcfunction2,v0,options);
      v=x;
and:

Wtot = fval;

So in short: The program in its current form minimises the amount of Work. That is wrong. It should minimise the amount of hydrogen spent. To do this I need to add the following: Work / time = power power in the formula to get the amount of hydrogen/per time Hydrogen per time * time = hydrogen minimise the hydrogen.

How to add this?