core.quat.norm¶

Inputs¶

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

Outputs¶

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

Parameters¶

Function has no parameters

State variables¶

Function has no state variables

Usage XML code snippet¶

core_quat_norm snippet for FLOW configuration file¶

<f name="norm" by_spec="core.quat.norm">
    <in alias="q">some_block_1/output</in>
</f>

Function’s artifacts¶

Declaration for core_quat_norm
Implementation for core_quat_norm
Header of core_quat_norm
Specification of core_quat_norm
Interface of core_quat_norm

declaration.py¶

from fspeclib import *


Function(
    name='core.quat.norm',
    title=LocalizedString(
        en='Normalize quaternion'
    ),

    inputs=[
        Input(
            name='q',
            title='Input quat',
            value_type='core.type.quat'
        ),
    ],

    outputs=[
        Input(
            name='q',
            title='Normalized quat',
            value_type='core.type.quat'
        ),
    ],
)

core_quat_norm_exec.c¶

#include <math.h>
#include "core_quat_norm.h"

#define square(s) ((s)*(s))

void core_quat_norm_exec(const core_quat_norm_inputs_t *i, core_quat_norm_outputs_t *o)
{

    core_type_f64_t n = 1.0 / sqrt( square(i->q.w) +  square(i->q.x) +  square(i->q.y) +  square(i->q.z));
    o->q.w = i->q.w * n;
    o->q.x = i->q.x * n;
    o->q.y = i->q.y * n;
    o->q.z = i->q.z * n;

}

core_quat_norm.h¶

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

#ifndef FSPEC_CORE_QUAT_NORM_H
#define FSPEC_CORE_QUAT_NORM_H

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

#include "function.h"

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

/**
 * @brief Inputs of `core.quat.norm` function
 */
typedef struct core_quat_norm_inputs_ {
    core_type_quat_t q;  /// Input quat

} core_quat_norm_inputs_t;

/**
 * @brief Outputs of `core.quat.norm` function
 */
typedef struct core_quat_norm_outputs_ {
    core_type_quat_t q; /// Normalized quat

} core_quat_norm_outputs_t;

typedef struct core_quat_norm_eswb_descriptors_ {
    eswb_topic_descr_t in_q;
    eswb_topic_descr_t out_all;
} core_quat_norm_eswb_descriptors_t;

typedef struct core_quat_norm_interface_ {
    core_quat_norm_inputs_t i;
    core_quat_norm_outputs_t o;
    core_quat_norm_eswb_descriptors_t eswb_descriptors;
} core_quat_norm_interface_t;

void core_quat_norm_exec(const core_quat_norm_inputs_t *i, core_quat_norm_outputs_t *o);

#endif // FSPEC_CORE_QUAT_NORM_H

core_quat_norm_spec.c¶

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

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

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

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

static const input_spec_t *inputs[2] = {
    &i_q,
    NULL
};

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

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

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

fspec_rv_t core_quat_norm_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_norm_call_exec(void *dh);

const function_spec_t atomic_core_quat_norm_spec = {
    .name = "core.quat.norm",
    .annotation = "Normalize quaternion",
    .inputs = inputs,
    .outputs = outputs,
    .params = params
};

const function_calls_t atomic_core_quat_norm_calls = {
    .interface_handle_size = sizeof(core_quat_norm_interface_t),
    .init = NULL,
    .init_inputs = core_quat_norm_call_init_inputs,
    .init_outputs = core_quat_norm_call_init_outputs,
    .pre_exec_init = NULL,
    .exec = core_quat_norm_call_exec,
    .set_params = NULL
};

const function_handler_t atomic_core_quat_norm_handler = {
    .spec = &atomic_core_quat_norm_spec,
    .calls = &atomic_core_quat_norm_calls,
    .extension_handler = NULL
};

core_quat_norm_interface.c¶

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

#include "core_quat_norm.h"

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

int core_quat_norm_interface_inputs_init(
    core_quat_norm_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_q = fspec_find_path2connect(conn_spec,"q");

    // 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++;
    }

    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_norm_interface_inputs_update(core_quat_norm_interface_t *interface)
{
    eswb_rv_t rv;

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

    return 0;
}

fspec_rv_t core_quat_norm_interface_outputs_init(
    core_quat_norm_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_norm_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_norm_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_norm_interface_outputs_update(core_quat_norm_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_norm_interface_update(core_quat_norm_interface_t *interface)
{
    core_quat_norm_interface_inputs_update(interface);
    core_quat_norm_exec(&interface->i, &interface->o);
    core_quat_norm_interface_outputs_update(interface);
}

fspec_rv_t core_quat_norm_call_init_inputs(void *dh, const func_conn_spec_t *conn_spec, eswb_topic_descr_t mounting_td)
{
    core_quat_norm_interface_t *interface = (core_quat_norm_interface_t*) dh;
    return core_quat_norm_interface_inputs_init(interface, conn_spec, mounting_td);
}

fspec_rv_t core_quat_norm_call_init_outputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
)
{
    core_quat_norm_interface_t *interface = (core_quat_norm_interface_t*) dh;
    return core_quat_norm_interface_outputs_init(interface, conn_spec, mounting_td, func_name);
}

void core_quat_norm_call_exec(void *dh)
{
    core_quat_norm_interface_t *interface = (core_quat_norm_interface_t*) dh;
    core_quat_norm_interface_update(interface);
}

← core.quat.from_euler core.quat.prop →