• Atomic functions catalog >
• core.quat.prop

core.quat.prop

Inputs

Name	Type	Title	Mandatory	Description
omega	<fspeclib.Structure object at 0x7f2b33a7cdf0>	Angular rate vector	True	None
q0	<fspeclib.Structure object at 0x7f2b33a7c250>	Initial quat	True	None
q	<fspeclib.Structure object at 0x7f2b33a7c250>	Recurrent quat	True	None
reset	core.type.bool	Reset	False	Command for re-initializing output quat by q0

Outputs

Name	Type	Title	Description
q	<fspeclib.Structure object at 0x7f2b33a7c250>	Updated quat	None

Parameters

Function has no parameters

State variables

Name	Type	Title	Description
inited	core.type.bool	Initialized flag	None

Usage XML code snippet

core_quat_prop snippet for FLOW configuration file

<f name="prop" by_spec="core.quat.prop">
    <in alias="omega">some_block_1/output</in>
    <in alias="q0">some_block_2/output</in>
    <in alias="q">some_block_3/output</in>
    <in alias="reset">some_block_4/output</in>   <!-- optional -->
</f>

Function’s artifacts

• Declaration for core_quat_prop
• Implementation for core_quat_prop
• Header of core_quat_prop
• Specification of core_quat_prop
• Interface of core_quat_prop

declaration.py

from fspeclib import *


Function(
    name='core.quat.prop',
    title=LocalizedString(
        en='Propagate quaternion'
    ),
    parameters=[

    ],
    inputs=[
        Input(
            name='omega',
            title='Angular rate vector',
            value_type='core.type.v3f64'
        ),

        Input(
            name='q0',
            title='Initial quat',
            value_type='core.type.quat'
        ),

        Input(
            name='q',
            title='Recurrent quat',
            value_type='core.type.quat'
        ),

        Input(
            name='reset',
            title='Reset',
            description='Command for re-initializing output quat by q0',
            value_type='core.type.bool',
            mandatory=False
        ),
    ],
    outputs=[
        Output(
            name='q',
            title='Updated quat',
            value_type='core.type.quat'
        ),
    ],
    state=[
        Variable(
            name='inited',
            title='Initialized flag',
            value_type='core.type.bool'
        ),
    ],

    injection=Injection(
        timedelta=True
    )
)

core_quat_prop_exec.c

#include "core_quat_prop.h"

void core_quat_prop_exec(
    const core_quat_prop_inputs_t *i,
    core_quat_prop_outputs_t *o,
    core_quat_prop_state_t *state,
    const core_quat_prop_injection_t *injection
)
{
    if (i->optional_inputs_flags.reset) {
        if (i->reset) {
            state->inited = 0;
        }
    }

    if (state->inited == FALSE) {
        o->q = i->q0;
        state->inited = 1;
    } else {
        o->q.w = i->q.w + -0.5 * ( i->q.x * i->omega.x + i->q.y * i->omega.y + i->q.z * i->omega.z ) * injection->dt;
        o->q.x = i->q.x +  0.5 * ( i->q.w * i->omega.x + i->q.y * i->omega.z - i->q.z * i->omega.y ) * injection->dt;
        o->q.y = i->q.y +  0.5 * ( i->q.w * i->omega.y + i->q.z * i->omega.x - i->q.x * i->omega.z ) * injection->dt;
        o->q.z = i->q.z +  0.5 * ( i->q.w * i->omega.z + i->q.x * i->omega.y - i->q.y * i->omega.x ) * injection->dt;
    }
}

core_quat_prop.h

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

#ifndef FSPEC_CORE_QUAT_PROP_H
#define FSPEC_CORE_QUAT_PROP_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"
#include "core_type_quat.h"
#include "core_type_v3f64.h"

/**
 * @brief Optional inputs connectivity flags structure for `core.quat.prop` function
 */
typedef struct {
    uint32_t reset:1;
} core_quat_prop_optional_inputs_flags_t;

/**
 * @brief Inputs of `core.quat.prop` function
 */
typedef struct core_quat_prop_inputs_ {
    core_type_v3f64_t omega;  /// Angular rate vector
    core_type_quat_t q0;  /// Initial quat
    core_type_quat_t q;  /// Recurrent quat
    core_type_bool_t reset;  /// Reset

    core_quat_prop_optional_inputs_flags_t optional_inputs_flags;
} core_quat_prop_inputs_t;

/**
 * @brief Outputs of `core.quat.prop` function
 */
typedef struct core_quat_prop_outputs_ {
    core_type_quat_t q; /// Updated quat

} core_quat_prop_outputs_t;

/**
 * @brief State variables of `core.quat.prop` function
 */
typedef struct core_quat_prop_state_ {
    core_type_bool_t inited;  /// Initialized flag
} core_quat_prop_state_t;

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

} core_quat_prop_injection_t;

/**
 * @brief Parameter flags of `core.quat.prop` function
 */
typedef struct core_quat_prop_params_flags_ {
} core_quat_prop_params_flags_t;

typedef struct core_quat_prop_eswb_descriptors_ {
    eswb_topic_descr_t in_omega;
    eswb_topic_descr_t in_q0;
    eswb_topic_descr_t in_q;
    eswb_topic_descr_t in_reset;
    eswb_topic_descr_t out_all;
} core_quat_prop_eswb_descriptors_t;

typedef struct core_quat_prop_interface_ {
    core_quat_prop_inputs_t i;
    core_quat_prop_outputs_t o;
    core_quat_prop_state_t state;
    core_quat_prop_injection_t injection;
    struct timespec prev_exec_time;
    int prev_exec_time_inited;
    core_quat_prop_eswb_descriptors_t eswb_descriptors;
} core_quat_prop_interface_t;

void core_quat_prop_exec(
    const core_quat_prop_inputs_t *i,
    core_quat_prop_outputs_t *o,
    core_quat_prop_state_t *state,
    const core_quat_prop_injection_t *injection
);

#endif // FSPEC_CORE_QUAT_PROP_H

core_quat_prop_spec.c

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

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

static const param_spec_t *params[1] = {
    NULL
};

static const input_spec_t i_omega = {
    .name = "omega",
    .annotation = "Angular rate vector",
    .flags = 0
};

static const input_spec_t i_q0 = {
    .name = "q0",
    .annotation = "Initial quat",
    .flags = 0
};

static const input_spec_t i_q = {
    .name = "q",
    .annotation = "Recurrent quat",
    .flags = 0
};

static const input_spec_t i_reset = {
    .name = "reset",
    .annotation = "Reset",
    .flags = 0
};

static const input_spec_t *inputs[5] = {
    &i_omega,
    &i_q0,
    &i_q,
    &i_reset,
    NULL
};

static const output_spec_t o_q = {
    .name = "q",
    .annotation = "Updated quat",
    .flags = 0
};

static const output_spec_t *outputs[2] = {
    &o_q,
    NULL
};

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

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

void core_quat_prop_call_exec(void *dh);

const function_spec_t atomic_core_quat_prop_spec = {
    .name = "core.quat.prop",
    .annotation = "Propagate quaternion",
    .inputs = inputs,
    .outputs = outputs,
    .params = params
};

const function_calls_t atomic_core_quat_prop_calls = {
    .interface_handle_size = sizeof(core_quat_prop_interface_t),
    .init = NULL,
    .init_inputs = core_quat_prop_call_init_inputs,
    .init_outputs = core_quat_prop_call_init_outputs,
    .pre_exec_init = NULL,
    .exec = core_quat_prop_call_exec,
    .set_params = NULL
};

const function_handler_t atomic_core_quat_prop_handler = {
    .spec = &atomic_core_quat_prop_spec,
    .calls = &atomic_core_quat_prop_calls,
    .extension_handler = NULL
};

core_quat_prop_interface.c

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

#include "core_quat_prop.h"

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

int core_quat_prop_interface_inputs_init(
    core_quat_prop_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_omega = fspec_find_path2connect(conn_spec,"omega");
    const char *topic_path_in_q0 = fspec_find_path2connect(conn_spec,"q0");
    const char *topic_path_in_q = fspec_find_path2connect(conn_spec,"q");
    const char *topic_path_in_reset = fspec_find_path2connect(conn_spec,"reset");

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

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

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

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

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

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

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

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

    return 0;
}

fspec_rv_t core_quat_prop_interface_outputs_init(
    core_quat_prop_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, 6);
    core_quat_prop_outputs_t out;
    eswb_rv_t rv;

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

    topic_proclaiming_tree_t* q_s_root = usr_topic_add_struct_child(cntx, rt, core_quat_prop_outputs_t, q, "q", tt_struct);
    usr_topic_add_struct_child(cntx, q_s_root, core_type_quat_t, w, "w", tt_double);
    usr_topic_add_struct_child(cntx, q_s_root, core_type_quat_t, x, "x", tt_double);
    usr_topic_add_struct_child(cntx, q_s_root, core_type_quat_t, y, "y", tt_double);
    usr_topic_add_struct_child(cntx, q_s_root, core_type_quat_t, z, "z", 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_quat_prop_interface_outputs_update(core_quat_prop_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_quat_prop_interface_update(core_quat_prop_interface_t *interface)
{
    core_quat_prop_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_quat_prop_exec(&interface->i, &interface->o, &interface->state, &interface->injection);
    core_quat_prop_interface_outputs_update(interface);
}

fspec_rv_t core_quat_prop_call_init_inputs(void *dh, const func_conn_spec_t *conn_spec, eswb_topic_descr_t mounting_td)
{
    core_quat_prop_interface_t *interface = (core_quat_prop_interface_t*) dh;
    return core_quat_prop_interface_inputs_init(interface, conn_spec, mounting_td);
}

fspec_rv_t core_quat_prop_call_init_outputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
)
{
    core_quat_prop_interface_t *interface = (core_quat_prop_interface_t*) dh;
    return core_quat_prop_interface_outputs_init(interface, conn_spec, mounting_td, func_name);
}

void core_quat_prop_call_exec(void *dh)
{
    core_quat_prop_interface_t *interface = (core_quat_prop_interface_t*) dh;
    core_quat_prop_interface_update(interface);
}

←   core.quat.norm core.quat.to_euler  →

© Catalyst Aerospace Technologies