hamnet70-gnuradio-legacy/gr-hamnet70/lib/pid_controller_impl.cc

108 lines
2.7 KiB
C++
Raw Normal View History

/* -*- c++ -*- */
/*
* Copyright 2019 Thomas Kolb.
*
* This is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3, or (at your option)
* any later version.
*
* This software is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software; see the file COPYING. If not, write to
* the Free Software Foundation, Inc., 51 Franklin Street,
* Boston, MA 02110-1301, USA.
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <gnuradio/io_signature.h>
#include "pid_controller_impl.h"
namespace gr {
namespace hamnet70 {
pid_controller::sptr
pid_controller::make(size_t interval, float p, float i, float d, float post_gain)
{
return gnuradio::get_initial_sptr
(new pid_controller_impl(interval, p, i, d, post_gain));
}
/*
* The private constructor
*/
pid_controller_impl::pid_controller_impl(size_t interval, float p, float i, float d, float post_gain)
: gr::block("pid_controller",
gr::io_signature::make(1, 1, sizeof(float)),
gr::io_signature::make(0, 0, 0)),
d_interval(interval),
d_lastInput(0),
d_iVal(0),
d_postGain(post_gain),
d_nextSampleIdx(0)
{
// TODO: scale with interval?
d_kp = p;
d_ki = i;
d_kd = d;
message_port_register_out(pmt::mp("control_value"));
}
/*
* Our virtual destructor.
*/
pid_controller_impl::~pid_controller_impl()
{
}
int
pid_controller_impl::general_work(int noutput_items,
gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
const float *in = (const float *) input_items[0];
int ninput_avail = ninput_items[0];
while(d_nextSampleIdx < ninput_avail) {
const float &inp = in[d_nextSampleIdx];
// proportional part
float control_value = d_kp * inp;
// integral part
d_iVal += d_ki * inp;
control_value += d_iVal;
// differential part
control_value += d_kd * (inp - d_lastInput);
d_lastInput = inp;
message_port_pub(pmt::intern("control_value"), pmt::from_double(d_postGain * control_value));
d_nextSampleIdx += d_interval;
}
d_nextSampleIdx -= ninput_avail;
consume(0, ninput_avail);
noutput_items = 0;
// Tell runtime system how many output items we produced.
return noutput_items;
}
} /* namespace hamnet70 */
} /* namespace gr */