blob: 00652282de375df61458a6664e6e00d5feada04c [file] [log] [blame]
James Feist22c257a2018-08-31 14:07:12 -07001/*
2// Copyright (c) 2018 Intel Corporation
3//
4// Licensed under the Apache License, Version 2.0 (the "License");
5// you may not use this file except in compliance with the License.
6// You may obtain a copy of the License at
7//
8// http://www.apache.org/licenses/LICENSE-2.0
9//
10// Unless required by applicable law or agreed to in writing, software
11// distributed under the License is distributed on an "AS IS" BASIS,
12// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13// See the License for the specific language governing permissions and
14// limitations under the License.
15*/
16
17#include "stepwise.hpp"
18
James Feist3dfaafd2018-09-20 15:46:58 -070019#include <cmath>
James Feist22c257a2018-08-31 14:07:12 -070020#include <cstddef>
21#include <limits>
22
Patrick Venturea0764872020-08-08 07:48:43 -070023namespace pid_control
24{
James Feist22c257a2018-08-31 14:07:12 -070025namespace ec
26{
Patrick Venturea0764872020-08-08 07:48:43 -070027
Patrick Venture5f59c0f2018-11-11 12:55:14 -080028double stepwise(const ec::StepwiseInfo& info, double input)
James Feist22c257a2018-08-31 14:07:12 -070029{
Patrick Venture5f59c0f2018-11-11 12:55:14 -080030 double value = info.output[0]; // if we are below the lowest
31 // point, we set the lowest value
James Feist22c257a2018-08-31 14:07:12 -070032
33 for (size_t ii = 1; ii < ec::maxStepwisePoints; ii++)
34 {
35
James Feist3dfaafd2018-09-20 15:46:58 -070036 if (std::isnan(info.reading[ii]))
James Feist22c257a2018-08-31 14:07:12 -070037 {
38 break;
39 }
40 if (info.reading[ii] > input)
41 {
42 break;
43 }
44 value = info.output[ii];
45 }
46
47 return value;
48}
Patrick Venturea0764872020-08-08 07:48:43 -070049
50} // namespace ec
51} // namespace pid_control