• Atomic functions catalog >
• core.cont.pid

core.cont.pid

Inputs

Name	Type	Title	Mandatory	Description
input	core.type.f64	Input	True	None
feedback	core.type.f64	Feedback	True	None
enable	core.type.bool	Enable	False	None
preset	core.type.f64	Preset source	False	Used to preset integral part at the moment of the PID enabling

Outputs

Name	Type	Title	Description
output	core.type.f64	Output	None
enabled	core.type.bool	Enabled	None

Parameters

Name	Type	Title	Mandatory	Default value	Description
Kp	core.type.f64	Proportional coefficient	False	1	None
Ki	core.type.f64	Integral coefficient	False	0	None
Kd	core.type.f64	Derivative coefficient	False	0	None
integral_min	core.type.f64	Lower bound of integral part of error	False	0	None
integral_max	core.type.f64	Upper bound of integral part of error	False	1	None
output_min	core.type.f64	Lower bound of controller output	False	0	None
output_max	core.type.f64	Upper bound of controller output	False	1	None

State variables

Name	Type	Title	Description
integral	core.type.f64	Accumulated Integral term	None
previous_error	core.type.f64	Previous error	None
time_from_last_iteration	core.type.f64	Time from last iteration	None
activated	core.type.bool	PID was activated before by enable signal	None

Usage XML code snippet

core_cont_pid snippet for FLOW configuration file

<f name="pid" by_spec="core.cont.pid">
    <in alias="input">some_block_1/output</in>
    <in alias="feedback">some_block_2/output</in>
    <in alias="enable">some_block_3/output</in>   <!-- optional -->
    <in alias="preset">some_block_4/output</in>   <!-- optional -->
    <param alias="Kp">0.0</param>   <!-- optional -->
    <param alias="Ki">0.0</param>   <!-- optional -->
    <param alias="Kd">0.0</param>   <!-- optional -->
    <param alias="integral_min">0.0</param>   <!-- optional -->
    <param alias="integral_max">0.0</param>   <!-- optional -->
    <param alias="output_min">0.0</param>   <!-- optional -->
    <param alias="output_max">0.0</param>   <!-- optional -->
</f>

Function’s artifacts

• Declaration for core_cont_pid
• Implementation for core_cont_pid
• Set parameters code for core_cont_pid
• Header of core_cont_pid
• Specification of core_cont_pid
• Other
  • Interface of core_cont_pid

declaration.py

from fspeclib import *


Function(
    name='core.cont.pid',
    title=LocalizedString(
        en='PID controller'
    ),
    parameters=[
        Parameter(
            name='Kp',
            title='Proportional coefficient',
            value_type='core.type.f64',
            tunable=True,
            default=1,
            constraints=[
                ThisValue() >= 0
            ]
        ),
        Parameter(
            name='Ki',
            title='Integral coefficient',
            value_type='core.type.f64',
            tunable=True,
            default=0,
            constraints=[
                ThisValue() >= 0
            ]
        ),
        Parameter(
            name='Kd',
            title='Derivative coefficient',
            value_type='core.type.f64',
            tunable=True,
            default=0,
            constraints=[
                ThisValue() >= 0
            ]
        ),
        Parameter(
            name='integral_min',
            title='Lower bound of integral part of error',
            value_type='core.type.f64',
            tunable=True,
            default=0,
            constraints=[
                ThisValue() <= 0
            ]
        ),
        Parameter(
            name='integral_max',
            title='Upper bound of integral part of error',
            value_type='core.type.f64',
            tunable=True,
            default=1,
            constraints=[
                ThisValue() >= 0
            ]
        ),
        Parameter(
            name='output_min',
            title='Lower bound of controller output',
            value_type='core.type.f64',
            tunable=True,
            default=0,
            constraints=[
                ThisValue() <= 0
            ]
        ),
        Parameter(
            name='output_max',
            title='Upper bound of controller output',
            value_type='core.type.f64',
            tunable=True,
            default=1,
            constraints=[
                ThisValue() >= 0,
            ]
        ),
    ],
    inputs=[
        Input(
            name='input',
            title='Input',
            value_type='core.type.f64'
        ),
        Input(
            name='feedback',
            title='Feedback',
            value_type='core.type.f64'
        ),
        Input(
            name='enable',
            title='Enable',
            value_type='core.type.bool',
            mandatory=False
        ),
        Input(
            name='preset',
            title='Preset source',
            description='Used to preset integral part at the moment of the PID enabling',
            value_type='core.type.f64',
            mandatory=False
        ),
    ],
    outputs=[
        Output(
            name='output',
            title='Output',
            value_type='core.type.f64'
        ),
        Output(
            name='enabled',
            title='Enabled',
            value_type='core.type.bool',
        )
    ],
    state=[
        Variable(
            name='integral',
            title='Accumulated Integral term',
            value_type='core.type.f64'
        ),
        Variable(
            name='previous_error',
            title='Previous error',
            value_type='core.type.f64'
        ),
        Variable(
            name='time_from_last_iteration',
            title='Time from last iteration',
            value_type='core.type.f64'
        ),
        Variable(
            name='activated',
            title='PID was activated before by enable signal',
            value_type='core.type.bool'
        )
    ],
    parameter_constraints=[
        ParameterValue('output_max') > ParameterValue('output_min'),
        ParameterValue('integral_max') > ParameterValue('integral_min')
    ],
    injection=Injection(
        timedelta=True
    )
)

core_cont_pid_exec.c

#include "core_cont_pid.h"

void core_cont_pid_exec(
        const core_cont_pid_inputs_t *i,
        core_cont_pid_outputs_t *o,
        const core_cont_pid_params_t *p,
        core_cont_pid_state_t *state,
        const core_cont_pid_injection_t *injection
)
{
    // if enable input is not connected PID is always enabled
    int pid_enabled = (i->optional_inputs_flags.enable && i->enable) || (!i->optional_inputs_flags.enable);

    if (pid_enabled) {
        double const dt = state->time_from_last_iteration + injection->dt;
        double const error = i->input - i->feedback;

        if (i->optional_inputs_flags.preset && !state->activated) {
            state->integral = i->preset;
            state->activated = TRUE;
        } else {
            state->integral += p->Ki *(0.5 * (error + state->previous_error)) * dt;
        }

        if (state->integral > p->integral_max) {
            state->integral = p->integral_max;
        } else if (state->integral < p->integral_min) {
            state->integral = p->integral_min;
        }

        double const P = p->Kp * error;
        double const I = state->integral;
        double const D = p->Kd * (error - state->previous_error) / dt;

        o->output = P + I + D;

        if (o->output > p->output_max) {
            o->output = p->output_max;
        } else if (o->output < p->output_min) {
            o->output = p->output_min;
        }

        o->enabled = TRUE;

        state->time_from_last_iteration = 0;
        state->previous_error = error;
    } else {
        o->enabled = FALSE;
        state->activated = FALSE;
    }

    // state->time_from_last_iteration += injection->dt; --> FIXME will need that after we will be able to "feel" input update event
}

core_cont_pid_set_params.c

#include "core_cont_pid.h"
#include "conv.h"

#include <string.h>

#include "error.h"

fspec_rv_t core_cont_pid_set_params(core_cont_pid_params_t *params, const func_param_t *param_pairs, int initial_call)
{
    // Get parameters
    core_cont_pid_params_t p = *params;

    core_cont_pid_params_flags_t flags;
    memset(&flags, 0, sizeof(flags));

    int violation_count = 0;
    conv_rv_t crv = conv_rv_ok;

    // Parse parameters
    for (int i = 0; param_pairs[i].alias != NULL; i++) {
        if (strcmp(param_pairs[i].alias, "Kp") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kp)) == conv_rv_ok) {
                flags.changed_param_Kp = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Ki") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Ki)) == conv_rv_ok) {
                flags.changed_param_Ki = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kd") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kd)) == conv_rv_ok) {
                flags.changed_param_Kd = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "integral_min") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.integral_min)) == conv_rv_ok) {
                flags.changed_param_integral_min = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "integral_max") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.integral_max)) == conv_rv_ok) {
                flags.changed_param_integral_max = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "output_min") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.output_min)) == conv_rv_ok) {
                flags.changed_param_output_min = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "output_max") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.output_max)) == conv_rv_ok) {
                flags.changed_param_output_max = 1;
            }
        } else {
            error("failed unsupported parameter '%s'", param_pairs[i].alias);
            violation_count ++;
        }

        if (crv != conv_rv_ok) {
            error("Parameter '%s' format error (%s)", param_pairs[i].alias, param_pairs[i].value);
            violation_count++;
        }
    }

    if (initial_call) {
        // Set default value for non mandatory parameters which are not set
        if (!flags.changed_param_Kp) {
            p.Kp = 1;
            flags.changed_param_Kp = 1;
        }
        if (!flags.changed_param_Ki) {
            p.Ki = 0;
            flags.changed_param_Ki = 1;
        }
        if (!flags.changed_param_Kd) {
            p.Kd = 0;
            flags.changed_param_Kd = 1;
        }
        if (!flags.changed_param_integral_min) {
            p.integral_min = 0;
            flags.changed_param_integral_min = 1;
        }
        if (!flags.changed_param_integral_max) {
            p.integral_max = 1;
            flags.changed_param_integral_max = 1;
        }
        if (!flags.changed_param_output_min) {
            p.output_min = 0;
            flags.changed_param_output_min = 1;
        }
        if (!flags.changed_param_output_max) {
            p.output_max = 1;
            flags.changed_param_output_max = 1;
        }
    }

    if (violation_count > 0) {
        return fspec_rv_inval_param;
    }

    // Validate parameters
    if (!(p.Kp >= 0)) {
        error("failed constraint (Kp >= 0)");
        return fspec_rv_inval_param;
    } else if (!(p.Ki >= 0)) {
        error("failed constraint (Ki >= 0)");
        return fspec_rv_inval_param;
    } else if (!(p.Kd >= 0)) {
        error("failed constraint (Kd >= 0)");
        return fspec_rv_inval_param;
    } else if (!(p.integral_min <= 0)) {
        error("failed constraint (integral_min <= 0)");
        return fspec_rv_inval_param;
    } else if (!(p.integral_max >= 0)) {
        error("failed constraint (integral_max >= 0)");
        return fspec_rv_inval_param;
    } else if (!(p.output_min <= 0)) {
        error("failed constraint (output_min <= 0)");
        return fspec_rv_inval_param;
    } else if (!(p.output_max >= 0)) {
        error("failed constraint (output_max >= 0)");
        return fspec_rv_inval_param;
    } else if (!(p.output_max > p.output_min)) {
        error("failed constraint (output_max > output_min)");
        return fspec_rv_inval_param;
    } else if (!(p.integral_max > p.integral_min)) {
        error("failed constraint (integral_max > integral_min)");
        return fspec_rv_inval_param;
    }

    // Set parameters
    *params = p;

    return fspec_rv_ok;
}

core_cont_pid.h

/**
 *  Automatically-generated file. Do not edit!
 */

#ifndef FSPEC_CORE_CONT_PID_H
#define FSPEC_CORE_CONT_PID_H

#include <stdint.h>
#include <eswb/types.h>

#include "function.h"

/* Include declaration of dependency types */
#include "core_type_bool.h"
#include "core_type_f64.h"

/**
 * @brief Parameters of `core.cont.pid` function
 */
typedef struct core_cont_pid_params_{
    core_type_f64_t Kp;  /// Proportional coefficient
    core_type_f64_t Ki;  /// Integral coefficient
    core_type_f64_t Kd;  /// Derivative coefficient
    core_type_f64_t integral_min;  /// Lower bound of integral part of error
    core_type_f64_t integral_max;  /// Upper bound of integral part of error
    core_type_f64_t output_min;  /// Lower bound of controller output
    core_type_f64_t output_max;  /// Upper bound of controller output
} core_cont_pid_params_t;

/**
 * @brief Optional inputs connectivity flags structure for `core.cont.pid` function
 */
typedef struct {
    uint32_t enable:1;
    uint32_t preset:1;
} core_cont_pid_optional_inputs_flags_t;

/**
 * @brief Inputs of `core.cont.pid` function
 */
typedef struct core_cont_pid_inputs_ {
    core_type_f64_t input;  /// Input
    core_type_f64_t feedback;  /// Feedback
    core_type_bool_t enable;  /// Enable
    core_type_f64_t preset;  /// Preset source

    core_cont_pid_optional_inputs_flags_t optional_inputs_flags;
} core_cont_pid_inputs_t;

/**
 * @brief Outputs of `core.cont.pid` function
 */
typedef struct core_cont_pid_outputs_ {
    core_type_f64_t output; /// Output

    core_type_bool_t enabled; /// Enabled

} core_cont_pid_outputs_t;

/**
 * @brief State variables of `core.cont.pid` function
 */
typedef struct core_cont_pid_state_ {
    core_type_f64_t integral;  /// Accumulated Integral term
    core_type_f64_t previous_error;  /// Previous error
    core_type_f64_t time_from_last_iteration;  /// Time from last iteration
    core_type_bool_t activated;  /// PID was activated before by enable signal
} core_cont_pid_state_t;

/**
 * @brief Injections for `core.cont.pid` function
 */
typedef struct core_cont_pid_injection_ {
    core_type_f64_t dt;  /// Time delta from previous iteration (measured in seconds)

} core_cont_pid_injection_t;

/**
 * @brief Parameter flags of `core.cont.pid` function
 */
typedef struct core_cont_pid_params_flags_ {
    uint64_t changed_param_Kp:1;
    uint64_t changed_param_Ki:1;
    uint64_t changed_param_Kd:1;
    uint64_t changed_param_integral_min:1;
    uint64_t changed_param_integral_max:1;
    uint64_t changed_param_output_min:1;
    uint64_t changed_param_output_max:1;
} core_cont_pid_params_flags_t;

typedef struct core_cont_pid_eswb_descriptors_ {
    eswb_topic_descr_t in_input;
    eswb_topic_descr_t in_feedback;
    eswb_topic_descr_t in_enable;
    eswb_topic_descr_t in_preset;
    eswb_topic_descr_t out_all;
} core_cont_pid_eswb_descriptors_t;

typedef struct core_cont_pid_interface_ {
    core_cont_pid_inputs_t i;
    core_cont_pid_outputs_t o;
    core_cont_pid_params_t p;
    core_cont_pid_state_t state;
    core_cont_pid_injection_t injection;
    struct timespec prev_exec_time;
    int prev_exec_time_inited;
    core_cont_pid_eswb_descriptors_t eswb_descriptors;
} core_cont_pid_interface_t;

fspec_rv_t core_cont_pid_set_params(core_cont_pid_params_t *params, const func_param_t *param_pairs, int initial_call);

void core_cont_pid_exec(
    const core_cont_pid_inputs_t *i,
    core_cont_pid_outputs_t *o,
    const core_cont_pid_params_t *p,
    core_cont_pid_state_t *state,
    const core_cont_pid_injection_t *injection
);

#endif // FSPEC_CORE_CONT_PID_H

core_cont_pid_spec.c

/**
 *  Automatically-generated file. Do not edit!
 */

#include "core_cont_pid.h"
#include <eswb/types.h>

static const param_spec_t param_Kp = {
    .name = "Kp",
    .default_value = "0.0",
    .annotation = "Proportional coefficient",
    .flags = 0
};

static const param_spec_t param_Ki = {
    .name = "Ki",
    .default_value = "0.0",
    .annotation = "Integral coefficient",
    .flags = 0
};

static const param_spec_t param_Kd = {
    .name = "Kd",
    .default_value = "0.0",
    .annotation = "Derivative coefficient",
    .flags = 0
};

static const param_spec_t param_integral_min = {
    .name = "integral_min",
    .default_value = "0.0",
    .annotation = "Lower bound of integral part of error",
    .flags = 0
};

static const param_spec_t param_integral_max = {
    .name = "integral_max",
    .default_value = "0.0",
    .annotation = "Upper bound of integral part of error",
    .flags = 0
};

static const param_spec_t param_output_min = {
    .name = "output_min",
    .default_value = "0.0",
    .annotation = "Lower bound of controller output",
    .flags = 0
};

static const param_spec_t param_output_max = {
    .name = "output_max",
    .default_value = "0.0",
    .annotation = "Upper bound of controller output",
    .flags = 0
};

static const param_spec_t *params[8] = {
    &param_Kp,
    &param_Ki,
    &param_Kd,
    &param_integral_min,
    &param_integral_max,
    &param_output_min,
    &param_output_max,
    NULL
};

static const input_spec_t i_input = {
    .name = "input",
    .annotation = "Input",
    .flags = 0
};

static const input_spec_t i_feedback = {
    .name = "feedback",
    .annotation = "Feedback",
    .flags = 0
};

static const input_spec_t i_enable = {
    .name = "enable",
    .annotation = "Enable",
    .flags = 0
};

static const input_spec_t i_preset = {
    .name = "preset",
    .annotation = "Preset source",
    .flags = 0
};

static const input_spec_t *inputs[5] = {
    &i_input,
    &i_feedback,
    &i_enable,
    &i_preset,
    NULL
};

static const output_spec_t o_output = {
    .name = "output",
    .annotation = "Output",
    .flags = 0
};

static const output_spec_t o_enabled = {
    .name = "enabled",
    .annotation = "Enabled",
    .flags = 0
};

static const output_spec_t *outputs[3] = {
    &o_output,
    &o_enabled,
    NULL
};

fspec_rv_t core_cont_pid_call_init_inputs(void *dh, const func_conn_spec_t *conn_spec, eswb_topic_descr_t mounting_td);

fspec_rv_t core_cont_pid_call_init_outputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
);

void core_cont_pid_call_exec(void *dh);

fspec_rv_t core_cont_pid_call_set_params(void *dh, const func_param_t *param_pairs, int initial_call);

const function_spec_t atomic_core_cont_pid_spec = {
    .name = "core.cont.pid",
    .annotation = "PID controller",
    .inputs = inputs,
    .outputs = outputs,
    .params = params
};

const function_calls_t atomic_core_cont_pid_calls = {
    .interface_handle_size = sizeof(core_cont_pid_interface_t),
    .init = NULL,
    .init_inputs = core_cont_pid_call_init_inputs,
    .init_outputs = core_cont_pid_call_init_outputs,
    .pre_exec_init = NULL,
    .exec = core_cont_pid_call_exec,
    .set_params = core_cont_pid_call_set_params
};

const function_handler_t atomic_core_cont_pid_handler = {
    .spec = &atomic_core_cont_pid_spec,
    .calls = &atomic_core_cont_pid_calls,
    .extension_handler = NULL
};

core_cont_pid_interface.c

/**
 *  Automatically-generated file. Do not edit!
 */

#include "core_cont_pid.h"

#include "error.h"
#include <eswb/api.h>
#include <eswb/topic_proclaiming_tree.h>
#include <eswb/errors.h>

int core_cont_pid_interface_inputs_init(
    core_cont_pid_interface_t *interface,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td
)
{
    eswb_rv_t rv;
    int errcnt_no_topic = 0;
    int errcnt_no_input = 0;
    const char *topic_path_in_input = fspec_find_path2connect(conn_spec,"input");
    const char *topic_path_in_feedback = fspec_find_path2connect(conn_spec,"feedback");
    const char *topic_path_in_enable = fspec_find_path2connect(conn_spec,"enable");
    const char *topic_path_in_preset = fspec_find_path2connect(conn_spec,"preset");

    // Connecting mandatory input "input"
    if (topic_path_in_input != NULL) {
        rv = eswb_connect_nested(mounting_td, topic_path_in_input, &interface->eswb_descriptors.in_input);
        if(rv != eswb_e_ok) {
            error("failed connect input \"input\" to topic \"%s\": %s", topic_path_in_input, eswb_strerror(rv));
            errcnt_no_topic++;
        }
    } else {
        error("mandatory input \"input\" is not speicifed");
        errcnt_no_input++;
    }

    // Connecting mandatory input "feedback"
    if (topic_path_in_feedback != NULL) {
        rv = eswb_connect_nested(mounting_td, topic_path_in_feedback, &interface->eswb_descriptors.in_feedback);
        if(rv != eswb_e_ok) {
            error("failed connect input \"feedback\" to topic \"%s\": %s", topic_path_in_feedback, eswb_strerror(rv));
            errcnt_no_topic++;
        }
    } else {
        error("mandatory input \"feedback\" is not speicifed");
        errcnt_no_input++;
    }

    // Connecting optional input "enable"
    if (topic_path_in_enable != NULL) {
        interface->i.optional_inputs_flags.enable = 1;
        rv = eswb_connect_nested(mounting_td, topic_path_in_enable, &interface->eswb_descriptors.in_enable);
        if(rv != eswb_e_ok) {
            error("failed connect input \"enable\" to topic \"%s\": %s", topic_path_in_enable, eswb_strerror(rv));
            errcnt_no_topic++;
        }
    }

    // Connecting optional input "preset"
    if (topic_path_in_preset != NULL) {
        interface->i.optional_inputs_flags.preset = 1;
        rv = eswb_connect_nested(mounting_td, topic_path_in_preset, &interface->eswb_descriptors.in_preset);
        if(rv != eswb_e_ok) {
            error("failed connect input \"preset\" to topic \"%s\": %s", topic_path_in_preset, eswb_strerror(rv));
            errcnt_no_topic++;
        }
    }

    if ((errcnt_no_input > 0) || (errcnt_no_topic > 0)) {
        if (errcnt_no_input > errcnt_no_topic) {
            return fspec_rv_no_input;
        } else {
            return fspec_rv_no_topic;
        }
    }
    return fspec_rv_ok;
}

fspec_rv_t core_cont_pid_interface_inputs_update(core_cont_pid_interface_t *interface)
{
    eswb_rv_t rv;

    rv = eswb_read(interface->eswb_descriptors.in_input, &interface->i.input);
    if(rv != eswb_e_ok) {
        /*FIXME nothing to do yet*/
    }

    rv = eswb_read(interface->eswb_descriptors.in_feedback, &interface->i.feedback);
    if(rv != eswb_e_ok) {
        /*FIXME nothing to do yet*/
    }

    if (interface->i.optional_inputs_flags.enable) {
        rv = eswb_read(interface->eswb_descriptors.in_enable, &interface->i.enable);
        if(rv != eswb_e_ok) {
            /*FIXME nothing to do yet*/
        }
    }

    if (interface->i.optional_inputs_flags.preset) {
        rv = eswb_read(interface->eswb_descriptors.in_preset, &interface->i.preset);
        if(rv != eswb_e_ok) {
            /*FIXME nothing to do yet*/
        }
    }

    return 0;
}

fspec_rv_t core_cont_pid_interface_outputs_init(
    core_cont_pid_interface_t *interface,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
)
{
    TOPIC_TREE_CONTEXT_LOCAL_DEFINE(cntx, 3);
    core_cont_pid_outputs_t out;
    eswb_rv_t rv;

    topic_proclaiming_tree_t *rt = usr_topic_set_struct(cntx, out, func_name);

    usr_topic_add_struct_child(cntx, rt, core_cont_pid_outputs_t, output, "output", tt_double);
    usr_topic_add_struct_child(cntx, rt, core_cont_pid_outputs_t, enabled, "enabled", tt_int32);
    rv = eswb_proclaim_tree(mounting_td, rt, cntx->t_num, &interface->eswb_descriptors.out_all);
    if (rv != eswb_e_ok) {
        return fspec_rv_publish_err;
    }

    return fspec_rv_ok;
}

fspec_rv_t core_cont_pid_interface_outputs_update(core_cont_pid_interface_t *interface)
{
    eswb_rv_t rv;

    rv = eswb_update_topic(interface->eswb_descriptors.out_all, &interface->o);
    if (rv != eswb_e_ok) {
        return 1;
    }

    return 0;
}

void core_cont_pid_interface_update(core_cont_pid_interface_t *interface)
{
    core_cont_pid_interface_inputs_update(interface);
    if (interface->prev_exec_time_inited) {
        function_getdeltatime(ft_monotonic, &interface->prev_exec_time, dtp_update_prev, &interface->injection.dt);
    } else {
        function_gettime(ft_monotonic, &interface->prev_exec_time);
        interface->prev_exec_time_inited = -1;
        interface->injection.dt = 0;
    }

    core_cont_pid_exec(&interface->i, &interface->o, &interface->p, &interface->state, &interface->injection);
    core_cont_pid_interface_outputs_update(interface);
}

fspec_rv_t core_cont_pid_call_set_params(void *dh, const func_param_t *param_pairs, int initial_call)
{
    core_cont_pid_interface_t *interface = (core_cont_pid_interface_t*) dh;
    return core_cont_pid_set_params(&interface->p, param_pairs, initial_call);
}

fspec_rv_t core_cont_pid_call_init_inputs(void *dh, const func_conn_spec_t *conn_spec, eswb_topic_descr_t mounting_td)
{
    core_cont_pid_interface_t *interface = (core_cont_pid_interface_t*) dh;
    return core_cont_pid_interface_inputs_init(interface, conn_spec, mounting_td);
}

fspec_rv_t core_cont_pid_call_init_outputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
)
{
    core_cont_pid_interface_t *interface = (core_cont_pid_interface_t*) dh;
    return core_cont_pid_interface_outputs_init(interface, conn_spec, mounting_td, func_name);
}

void core_cont_pid_call_exec(void *dh)
{
    core_cont_pid_interface_t *interface = (core_cont_pid_interface_t*) dh;
    core_cont_pid_interface_update(interface);
}

←   core.cont.mux3f64 core.cont.quadrotor_mix  →

© Catalyst Aerospace Technologies