• Atomic functions catalog >
• core.cont.quadrotor_mix

core.cont.quadrotor_mix

Inputs

Name	Type	Title	Mandatory	Description
transv	core.type.f64	Transv	True	None
longit	core.type.f64	Longit	True	None
rudder	core.type.f64	Rudder	False	None
collective	core.type.f64	Collective	False	None

Outputs

Name	Type	Title	Description
m1	core.type.f64	Motor 1	Motor 1 ESC output
m2	core.type.f64	Motor 2	Motor 2 ESC output
m3	core.type.f64	Motor 3	Motor 3 ESC output
m4	core.type.f64	Motor 4	Motor 4 ESC output

Parameters

Name	Type	Title	Mandatory	Default value	Description
Kt_m1	core.type.f64	Transversal control weight for Motor 1	True	None	None
Kt_m2	core.type.f64	Transversal control weight for Motor 2	True	None	None
Kt_m3	core.type.f64	Transversal control weight for Motor 3	True	None	None
Kt_m4	core.type.f64	Transversal control weight for Motor 4	True	None	None
Kl_m1	core.type.f64	Longitudal control weight Motor 1	True	None	None
Kl_m2	core.type.f64	Longitudal control weight Motor 2	True	None	None
Kl_m3	core.type.f64	Longitudal control weight Motor 3	True	None	None
Kl_m4	core.type.f64	Longitudal control weight Motor 4	True	None	None
Kc	core.type.f64	Collective control weight for all motors	True	None	None
Kr_m1	core.type.f64	Rudder control weight for Motor 1	True	None	None
Kr_m2	core.type.f64	Rudder control weight for Motor 2	True	None	None
Kr_m3	core.type.f64	Rudder control weight for Motor 3	True	None	None
Kr_m4	core.type.f64	Rudder control weight for Motor 4	True	None	None

State variables

Function has no state variables

Usage XML code snippet

core_cont_quadrotor_mix snippet for FLOW configuration file

<f name="quadrotor_mix" by_spec="core.cont.quadrotor_mix">
    <in alias="transv">some_block_1/output</in>
    <in alias="longit">some_block_2/output</in>
    <in alias="rudder">some_block_3/output</in>   <!-- optional -->
    <in alias="collective">some_block_4/output</in>   <!-- optional -->
    <param alias="Kt_m1">0.0</param>
    <param alias="Kt_m2">0.0</param>
    <param alias="Kt_m3">0.0</param>
    <param alias="Kt_m4">0.0</param>
    <param alias="Kl_m1">0.0</param>
    <param alias="Kl_m2">0.0</param>
    <param alias="Kl_m3">0.0</param>
    <param alias="Kl_m4">0.0</param>
    <param alias="Kc">0.0</param>
    <param alias="Kr_m1">0.0</param>
    <param alias="Kr_m2">0.0</param>
    <param alias="Kr_m3">0.0</param>
    <param alias="Kr_m4">0.0</param>
</f>

Function’s artifacts

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

declaration.py

from fspeclib import *


Function(
    name='core.cont.quadrotor_mix',
    title=LocalizedString(
        en='Mixer for the classic X quad-rotor control signal'
    ),
    parameters=[
        Parameter(
            name='Kt_m1',
            title='Transversal control weight for Motor 1',
            value_type='core.type.f64',
        ),
        Parameter(
            name='Kt_m2',
            title='Transversal control weight for Motor 2',
            value_type='core.type.f64',
        ),
        Parameter(
            name='Kt_m3',
            title='Transversal control weight for Motor 3',
            value_type='core.type.f64',
        ),
        Parameter(
            name='Kt_m4',
            title='Transversal control weight for Motor 4',
            value_type='core.type.f64',
        ),

        Parameter(
            name='Kl_m1',
            title='Longitudal control weight Motor 1',
            value_type='core.type.f64',
        ),

        Parameter(
            name='Kl_m2',
            title='Longitudal control weight Motor 2',
            value_type='core.type.f64',
        ),

        Parameter(
            name='Kl_m3',
            title='Longitudal control weight Motor 3',
            value_type='core.type.f64',
        ),

        Parameter(
            name='Kl_m4',
            title='Longitudal control weight Motor 4',
            value_type='core.type.f64',
        ),

        Parameter(
            name='Kc',
            title='Collective control weight for all motors',
            value_type='core.type.f64',
        ),

        Parameter(
            name='Kr_m1',
            title='Rudder control weight for Motor 1',
            value_type='core.type.f64',
        ),
        Parameter(
            name='Kr_m2',
            title='Rudder control weight for Motor 2',
            value_type='core.type.f64',
        ),
        Parameter(
            name='Kr_m3',
            title='Rudder control weight for Motor 3',
            value_type='core.type.f64',
        ),
        Parameter(
            name='Kr_m4',
            title='Rudder control weight for Motor 4',
            value_type='core.type.f64',
        ),

    ],
    inputs=[
        Input(
            name='transv',
            title='Transv',
            value_type='core.type.f64'
        ),
        Input(
            name='longit',
            title='Longit',
            value_type='core.type.f64'
        ),
        Input(
            name='rudder',
            title='Rudder',
            value_type='core.type.f64',
            mandatory=False
        ),
        Input(
            name='collective',
            title='Collective',
            value_type='core.type.f64',
            mandatory=False
        ),
    ],
    outputs=[
        Output(
            name='m1',
            title='Motor 1',
            description='Motor 1 ESC output',
            value_type='core.type.f64'
        ),
        Output(
            name='m2',
            title='Motor 2',
            description='Motor 2 ESC output',
            value_type='core.type.f64'
        ),
        Output(
            name='m3',
            title='Motor 3',
            description='Motor 3 ESC output',
            value_type='core.type.f64'
        ),
        Output(
            name='m4',
            title='Motor 4',
            description='Motor 4 ESC output',
            value_type='core.type.f64'
        ),
    ],

)

core_cont_quadrotor_mix_exec.c

#include "core_cont_quadrotor_mix.h"

static inline core_type_f64_t mix(
        core_type_f64_t t, core_type_f64_t l, core_type_f64_t c, core_type_f64_t r,
        core_type_f64_t kt, core_type_f64_t kl, core_type_f64_t kc, core_type_f64_t kr
        ) {

    core_type_f64_t out = t * kt + l * kl + c * kc + r * kr;
    out = out < 1.0 ? out : 1.0;
    out = out > 0.0 ? out : 0.0;

    return out;
}

void core_cont_quadrotor_mix_exec(
    const core_cont_quadrotor_mix_inputs_t *i,
    core_cont_quadrotor_mix_outputs_t *o,
    const core_cont_quadrotor_mix_params_t *p
)
{
    o->m1 = mix(i->transv, i->longit, i->collective, i->rudder,
                p->Kt_m1, p->Kl_m1, p->Kc, p->Kr_m1);
    o->m2 = mix(i->transv, i->longit, i->collective, i->rudder,
                p->Kt_m2, p->Kl_m2, p->Kc, p->Kr_m2);
    o->m3 = mix(i->transv, i->longit, i->collective, i->rudder,
                p->Kt_m3, p->Kl_m3, p->Kc, p->Kr_m3);
    o->m4 = mix(i->transv, i->longit, i->collective, i->rudder,
                p->Kt_m4, p->Kl_m4, p->Kc, p->Kr_m4);
}

core_cont_quadrotor_mix_set_params.c

#include "core_cont_quadrotor_mix.h"
#include "conv.h"

#include <string.h>

#include "error.h"

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

    core_cont_quadrotor_mix_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, "Kt_m1") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kt_m1)) == conv_rv_ok) {
                flags.changed_param_Kt_m1 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kt_m2") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kt_m2)) == conv_rv_ok) {
                flags.changed_param_Kt_m2 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kt_m3") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kt_m3)) == conv_rv_ok) {
                flags.changed_param_Kt_m3 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kt_m4") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kt_m4)) == conv_rv_ok) {
                flags.changed_param_Kt_m4 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kl_m1") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kl_m1)) == conv_rv_ok) {
                flags.changed_param_Kl_m1 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kl_m2") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kl_m2)) == conv_rv_ok) {
                flags.changed_param_Kl_m2 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kl_m3") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kl_m3)) == conv_rv_ok) {
                flags.changed_param_Kl_m3 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kl_m4") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kl_m4)) == conv_rv_ok) {
                flags.changed_param_Kl_m4 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kc") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kc)) == conv_rv_ok) {
                flags.changed_param_Kc = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kr_m1") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kr_m1)) == conv_rv_ok) {
                flags.changed_param_Kr_m1 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kr_m2") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kr_m2)) == conv_rv_ok) {
                flags.changed_param_Kr_m2 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kr_m3") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kr_m3)) == conv_rv_ok) {
                flags.changed_param_Kr_m3 = 1;
            }
        } else if (strcmp(param_pairs[i].alias, "Kr_m4") == 0) {
            if ((crv = conv_str_double(param_pairs[i].value, &p.Kr_m4)) == conv_rv_ok) {
                flags.changed_param_Kr_m4 = 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) {
        // Check all mandatory parameters are set
        if (!flags.changed_param_Kt_m1) {
            error("failed missed mandatory parameter 'Kt_m1'");
            violation_count ++;
        }
        if (!flags.changed_param_Kt_m2) {
            error("failed missed mandatory parameter 'Kt_m2'");
            violation_count ++;
        }
        if (!flags.changed_param_Kt_m3) {
            error("failed missed mandatory parameter 'Kt_m3'");
            violation_count ++;
        }
        if (!flags.changed_param_Kt_m4) {
            error("failed missed mandatory parameter 'Kt_m4'");
            violation_count ++;
        }
        if (!flags.changed_param_Kl_m1) {
            error("failed missed mandatory parameter 'Kl_m1'");
            violation_count ++;
        }
        if (!flags.changed_param_Kl_m2) {
            error("failed missed mandatory parameter 'Kl_m2'");
            violation_count ++;
        }
        if (!flags.changed_param_Kl_m3) {
            error("failed missed mandatory parameter 'Kl_m3'");
            violation_count ++;
        }
        if (!flags.changed_param_Kl_m4) {
            error("failed missed mandatory parameter 'Kl_m4'");
            violation_count ++;
        }
        if (!flags.changed_param_Kc) {
            error("failed missed mandatory parameter 'Kc'");
            violation_count ++;
        }
        if (!flags.changed_param_Kr_m1) {
            error("failed missed mandatory parameter 'Kr_m1'");
            violation_count ++;
        }
        if (!flags.changed_param_Kr_m2) {
            error("failed missed mandatory parameter 'Kr_m2'");
            violation_count ++;
        }
        if (!flags.changed_param_Kr_m3) {
            error("failed missed mandatory parameter 'Kr_m3'");
            violation_count ++;
        }
        if (!flags.changed_param_Kr_m4) {
            error("failed missed mandatory parameter 'Kr_m4'");
            violation_count ++;
        }
    }

    if (violation_count > 0) {
        return fspec_rv_inval_param;
    }

    // Set parameters
    *params = p;

    return fspec_rv_ok;
}

core_cont_quadrotor_mix.h

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

#ifndef FSPEC_CORE_CONT_QUADROTOR_MIX_H
#define FSPEC_CORE_CONT_QUADROTOR_MIX_H

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

#include "function.h"

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

/**
 * @brief Parameters of `core.cont.quadrotor_mix` function
 */
typedef struct core_cont_quadrotor_mix_params_{
    core_type_f64_t Kt_m1;  /// Transversal control weight for Motor 1
    core_type_f64_t Kt_m2;  /// Transversal control weight for Motor 2
    core_type_f64_t Kt_m3;  /// Transversal control weight for Motor 3
    core_type_f64_t Kt_m4;  /// Transversal control weight for Motor 4
    core_type_f64_t Kl_m1;  /// Longitudal control weight Motor 1
    core_type_f64_t Kl_m2;  /// Longitudal control weight Motor 2
    core_type_f64_t Kl_m3;  /// Longitudal control weight Motor 3
    core_type_f64_t Kl_m4;  /// Longitudal control weight Motor 4
    core_type_f64_t Kc;  /// Collective control weight for all motors
    core_type_f64_t Kr_m1;  /// Rudder control weight for Motor 1
    core_type_f64_t Kr_m2;  /// Rudder control weight for Motor 2
    core_type_f64_t Kr_m3;  /// Rudder control weight for Motor 3
    core_type_f64_t Kr_m4;  /// Rudder control weight for Motor 4
} core_cont_quadrotor_mix_params_t;

/**
 * @brief Optional inputs connectivity flags structure for `core.cont.quadrotor_mix` function
 */
typedef struct {
    uint32_t rudder:1;
    uint32_t collective:1;
} core_cont_quadrotor_mix_optional_inputs_flags_t;

/**
 * @brief Inputs of `core.cont.quadrotor_mix` function
 */
typedef struct core_cont_quadrotor_mix_inputs_ {
    core_type_f64_t transv;  /// Transv
    core_type_f64_t longit;  /// Longit
    core_type_f64_t rudder;  /// Rudder
    core_type_f64_t collective;  /// Collective

    core_cont_quadrotor_mix_optional_inputs_flags_t optional_inputs_flags;
} core_cont_quadrotor_mix_inputs_t;

/**
 * @brief Outputs of `core.cont.quadrotor_mix` function
 */
typedef struct core_cont_quadrotor_mix_outputs_ {
    core_type_f64_t m1; /// Motor 1

    core_type_f64_t m2; /// Motor 2

    core_type_f64_t m3; /// Motor 3

    core_type_f64_t m4; /// Motor 4

} core_cont_quadrotor_mix_outputs_t;

/**
 * @brief Parameter flags of `core.cont.quadrotor_mix` function
 */
typedef struct core_cont_quadrotor_mix_params_flags_ {
    uint64_t changed_param_Kt_m1:1;
    uint64_t changed_param_Kt_m2:1;
    uint64_t changed_param_Kt_m3:1;
    uint64_t changed_param_Kt_m4:1;
    uint64_t changed_param_Kl_m1:1;
    uint64_t changed_param_Kl_m2:1;
    uint64_t changed_param_Kl_m3:1;
    uint64_t changed_param_Kl_m4:1;
    uint64_t changed_param_Kc:1;
    uint64_t changed_param_Kr_m1:1;
    uint64_t changed_param_Kr_m2:1;
    uint64_t changed_param_Kr_m3:1;
    uint64_t changed_param_Kr_m4:1;
} core_cont_quadrotor_mix_params_flags_t;

typedef struct core_cont_quadrotor_mix_eswb_descriptors_ {
    eswb_topic_descr_t in_transv;
    eswb_topic_descr_t in_longit;
    eswb_topic_descr_t in_rudder;
    eswb_topic_descr_t in_collective;
    eswb_topic_descr_t out_all;
} core_cont_quadrotor_mix_eswb_descriptors_t;

typedef struct core_cont_quadrotor_mix_interface_ {
    core_cont_quadrotor_mix_inputs_t i;
    core_cont_quadrotor_mix_outputs_t o;
    core_cont_quadrotor_mix_params_t p;
    core_cont_quadrotor_mix_eswb_descriptors_t eswb_descriptors;
} core_cont_quadrotor_mix_interface_t;

fspec_rv_t core_cont_quadrotor_mix_set_params(
    core_cont_quadrotor_mix_params_t *params,
    const func_param_t *param_pairs,
    int initial_call
);

void core_cont_quadrotor_mix_exec(
    const core_cont_quadrotor_mix_inputs_t *i,
    core_cont_quadrotor_mix_outputs_t *o,
    const core_cont_quadrotor_mix_params_t *p
);

#endif // FSPEC_CORE_CONT_QUADROTOR_MIX_H

core_cont_quadrotor_mix_spec.c

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

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

static const param_spec_t param_Kt_m1 = {
    .name = "Kt_m1",
    .default_value = "0.0",
    .annotation = "Transversal control weight for Motor 1",
    .flags = 0
};

static const param_spec_t param_Kt_m2 = {
    .name = "Kt_m2",
    .default_value = "0.0",
    .annotation = "Transversal control weight for Motor 2",
    .flags = 0
};

static const param_spec_t param_Kt_m3 = {
    .name = "Kt_m3",
    .default_value = "0.0",
    .annotation = "Transversal control weight for Motor 3",
    .flags = 0
};

static const param_spec_t param_Kt_m4 = {
    .name = "Kt_m4",
    .default_value = "0.0",
    .annotation = "Transversal control weight for Motor 4",
    .flags = 0
};

static const param_spec_t param_Kl_m1 = {
    .name = "Kl_m1",
    .default_value = "0.0",
    .annotation = "Longitudal control weight Motor 1",
    .flags = 0
};

static const param_spec_t param_Kl_m2 = {
    .name = "Kl_m2",
    .default_value = "0.0",
    .annotation = "Longitudal control weight Motor 2",
    .flags = 0
};

static const param_spec_t param_Kl_m3 = {
    .name = "Kl_m3",
    .default_value = "0.0",
    .annotation = "Longitudal control weight Motor 3",
    .flags = 0
};

static const param_spec_t param_Kl_m4 = {
    .name = "Kl_m4",
    .default_value = "0.0",
    .annotation = "Longitudal control weight Motor 4",
    .flags = 0
};

static const param_spec_t param_Kc = {
    .name = "Kc",
    .default_value = "0.0",
    .annotation = "Collective control weight for all motors",
    .flags = 0
};

static const param_spec_t param_Kr_m1 = {
    .name = "Kr_m1",
    .default_value = "0.0",
    .annotation = "Rudder control weight for Motor 1",
    .flags = 0
};

static const param_spec_t param_Kr_m2 = {
    .name = "Kr_m2",
    .default_value = "0.0",
    .annotation = "Rudder control weight for Motor 2",
    .flags = 0
};

static const param_spec_t param_Kr_m3 = {
    .name = "Kr_m3",
    .default_value = "0.0",
    .annotation = "Rudder control weight for Motor 3",
    .flags = 0
};

static const param_spec_t param_Kr_m4 = {
    .name = "Kr_m4",
    .default_value = "0.0",
    .annotation = "Rudder control weight for Motor 4",
    .flags = 0
};

static const param_spec_t *params[14] = {
    &param_Kt_m1,
    &param_Kt_m2,
    &param_Kt_m3,
    &param_Kt_m4,
    &param_Kl_m1,
    &param_Kl_m2,
    &param_Kl_m3,
    &param_Kl_m4,
    &param_Kc,
    &param_Kr_m1,
    &param_Kr_m2,
    &param_Kr_m3,
    &param_Kr_m4,
    NULL
};

static const input_spec_t i_transv = {
    .name = "transv",
    .annotation = "Transv",
    .flags = 0
};

static const input_spec_t i_longit = {
    .name = "longit",
    .annotation = "Longit",
    .flags = 0
};

static const input_spec_t i_rudder = {
    .name = "rudder",
    .annotation = "Rudder",
    .flags = 0
};

static const input_spec_t i_collective = {
    .name = "collective",
    .annotation = "Collective",
    .flags = 0
};

static const input_spec_t *inputs[5] = {
    &i_transv,
    &i_longit,
    &i_rudder,
    &i_collective,
    NULL
};

static const output_spec_t o_m1 = {
    .name = "m1",
    .annotation = "Motor 1",
    .flags = 0
};

static const output_spec_t o_m2 = {
    .name = "m2",
    .annotation = "Motor 2",
    .flags = 0
};

static const output_spec_t o_m3 = {
    .name = "m3",
    .annotation = "Motor 3",
    .flags = 0
};

static const output_spec_t o_m4 = {
    .name = "m4",
    .annotation = "Motor 4",
    .flags = 0
};

static const output_spec_t *outputs[5] = {
    &o_m1,
    &o_m2,
    &o_m3,
    &o_m4,
    NULL
};

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

fspec_rv_t core_cont_quadrotor_mix_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_quadrotor_mix_call_exec(void *dh);

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

const function_spec_t atomic_core_cont_quadrotor_mix_spec = {
    .name = "core.cont.quadrotor_mix",
    .annotation = "Mixer for the classic X quad-rotor control signal",
    .inputs = inputs,
    .outputs = outputs,
    .params = params
};

const function_calls_t atomic_core_cont_quadrotor_mix_calls = {
    .interface_handle_size = sizeof(core_cont_quadrotor_mix_interface_t),
    .init = NULL,
    .init_inputs = core_cont_quadrotor_mix_call_init_inputs,
    .init_outputs = core_cont_quadrotor_mix_call_init_outputs,
    .pre_exec_init = NULL,
    .exec = core_cont_quadrotor_mix_call_exec,
    .set_params = core_cont_quadrotor_mix_call_set_params
};

const function_handler_t atomic_core_cont_quadrotor_mix_handler = {
    .spec = &atomic_core_cont_quadrotor_mix_spec,
    .calls = &atomic_core_cont_quadrotor_mix_calls,
    .extension_handler = NULL
};

core_cont_quadrotor_mix_interface.c

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

#include "core_cont_quadrotor_mix.h"

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

int core_cont_quadrotor_mix_interface_inputs_init(
    core_cont_quadrotor_mix_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_transv = fspec_find_path2connect(conn_spec,"transv");
    const char *topic_path_in_longit = fspec_find_path2connect(conn_spec,"longit");
    const char *topic_path_in_rudder = fspec_find_path2connect(conn_spec,"rudder");
    const char *topic_path_in_collective = fspec_find_path2connect(conn_spec,"collective");

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

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

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

    // Connecting optional input "collective"
    if (topic_path_in_collective != NULL) {
        interface->i.optional_inputs_flags.collective = 1;
        rv = eswb_connect_nested(mounting_td, topic_path_in_collective, &interface->eswb_descriptors.in_collective);
        if(rv != eswb_e_ok) {
            error("failed connect input \"collective\" to topic \"%s\": %s", topic_path_in_collective, 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_quadrotor_mix_interface_inputs_update(core_cont_quadrotor_mix_interface_t *interface)
{
    eswb_rv_t rv;

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

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

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

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

    return 0;
}

fspec_rv_t core_cont_quadrotor_mix_interface_outputs_init(
    core_cont_quadrotor_mix_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, 5);
    core_cont_quadrotor_mix_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_quadrotor_mix_outputs_t, m1, "m1", tt_double);
    usr_topic_add_struct_child(cntx, rt, core_cont_quadrotor_mix_outputs_t, m2, "m2", tt_double);
    usr_topic_add_struct_child(cntx, rt, core_cont_quadrotor_mix_outputs_t, m3, "m3", tt_double);
    usr_topic_add_struct_child(cntx, rt, core_cont_quadrotor_mix_outputs_t, m4, "m4", tt_double);
    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_quadrotor_mix_interface_outputs_update(core_cont_quadrotor_mix_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_quadrotor_mix_interface_update(core_cont_quadrotor_mix_interface_t *interface)
{
    core_cont_quadrotor_mix_interface_inputs_update(interface);
    core_cont_quadrotor_mix_exec(&interface->i, &interface->o, &interface->p);
    core_cont_quadrotor_mix_interface_outputs_update(interface);
}

fspec_rv_t core_cont_quadrotor_mix_call_set_params(void *dh, const func_param_t *param_pairs, int initial_call)
{
    core_cont_quadrotor_mix_interface_t *interface = (core_cont_quadrotor_mix_interface_t*) dh;
    return core_cont_quadrotor_mix_set_params(&interface->p, param_pairs, initial_call);
}

fspec_rv_t core_cont_quadrotor_mix_call_init_inputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td
)
{
    core_cont_quadrotor_mix_interface_t *interface = (core_cont_quadrotor_mix_interface_t*) dh;
    return core_cont_quadrotor_mix_interface_inputs_init(interface, conn_spec, mounting_td);
}

fspec_rv_t core_cont_quadrotor_mix_call_init_outputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
)
{
    core_cont_quadrotor_mix_interface_t *interface = (core_cont_quadrotor_mix_interface_t*) dh;
    return core_cont_quadrotor_mix_interface_outputs_init(interface, conn_spec, mounting_td, func_name);
}

void core_cont_quadrotor_mix_call_exec(void *dh)
{
    core_cont_quadrotor_mix_interface_t *interface = (core_cont_quadrotor_mix_interface_t*) dh;
    core_cont_quadrotor_mix_interface_update(interface);
}

←   core.cont.pid core.filter.median  →

© Catalyst Aerospace Technologies