core.quat.from_euler¶

Inputs¶

Name	Type	Title	Mandatory	Description
roll	core.type.f64	Roll	True	None
pitch	core.type.f64	Pitch	True	None
yaw	core.type.f64	Yaw	True	None

Outputs¶

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

Parameters¶

Function has no parameters

State variables¶

Function has no state variables

Usage XML code snippet¶

core_quat_from_euler snippet for FLOW configuration file¶

<f name="from_euler" by_spec="core.quat.from_euler">
    <in alias="roll">some_block_1/output</in>
    <in alias="pitch">some_block_2/output</in>
    <in alias="yaw">some_block_3/output</in>
</f>

Function’s artifacts¶

Declaration for core_quat_from_euler
Implementation for core_quat_from_euler
Header of core_quat_from_euler
Specification of core_quat_from_euler
Interface of core_quat_from_euler

declaration.py¶

from fspeclib import *


Function(
    name='core.quat.from_euler',
    title=LocalizedString(
        en='Covert Euler angles to quaternion'
    ),
    inputs=[
        Input(
            name='roll',
            title='Roll',
            value_type='core.type.f64'
        ),
        Input(
            name='pitch',
            title='Pitch',
            value_type='core.type.f64'
        ),
        Input(
            name='yaw',
            title='Yaw',
            value_type='core.type.f64'
        ),
    ],
    outputs=[
        Output(
            name='q',
            title='Converted quat',
            value_type='core.type.quat'
        ),
    ],
)

core_quat_from_euler_exec.c¶

#include <math.h>

#include "core_quat_from_euler.h"

void core_quat_from_euler_exec(const core_quat_from_euler_inputs_t *i, core_quat_from_euler_outputs_t *o)
{
    float sin_roll  = sinf(i->roll / 2 );
    float cos_roll  = cosf(i->roll / 2 );
    float sin_pitch= sinf(i->pitch / 2 );
    float cos_pitch= cosf(i->pitch / 2 );
    float cos_yaw   = cosf(i->yaw / 2 );
    float sin_yaw   = sinf(i->yaw / 2 );

    o->q.w = cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw;
    o->q.x = sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw;
    o->q.y = cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw;
    o->q.z = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw;

    // TODO cascade update e.g. for initial setup of the quat
}

core_quat_from_euler.h¶

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

#ifndef FSPEC_CORE_QUAT_FROM_EULER_H
#define FSPEC_CORE_QUAT_FROM_EULER_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.from_euler` function
 */
typedef struct core_quat_from_euler_inputs_ {
    core_type_f64_t roll;  /// Roll
    core_type_f64_t pitch;  /// Pitch
    core_type_f64_t yaw;  /// Yaw

} core_quat_from_euler_inputs_t;

/**
 * @brief Outputs of `core.quat.from_euler` function
 */
typedef struct core_quat_from_euler_outputs_ {
    core_type_quat_t q; /// Converted quat

} core_quat_from_euler_outputs_t;

typedef struct core_quat_from_euler_eswb_descriptors_ {
    eswb_topic_descr_t in_roll;
    eswb_topic_descr_t in_pitch;
    eswb_topic_descr_t in_yaw;
    eswb_topic_descr_t out_all;
} core_quat_from_euler_eswb_descriptors_t;

typedef struct core_quat_from_euler_interface_ {
    core_quat_from_euler_inputs_t i;
    core_quat_from_euler_outputs_t o;
    core_quat_from_euler_eswb_descriptors_t eswb_descriptors;
} core_quat_from_euler_interface_t;

void core_quat_from_euler_exec(const core_quat_from_euler_inputs_t *i, core_quat_from_euler_outputs_t *o);

#endif // FSPEC_CORE_QUAT_FROM_EULER_H

core_quat_from_euler_spec.c¶

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

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

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

static const input_spec_t i_roll = {
    .name = "roll",
    .annotation = "Roll",
    .flags = 0
};

static const input_spec_t i_pitch = {
    .name = "pitch",
    .annotation = "Pitch",
    .flags = 0
};

static const input_spec_t i_yaw = {
    .name = "yaw",
    .annotation = "Yaw",
    .flags = 0
};

static const input_spec_t *inputs[4] = {
    &i_roll,
    &i_pitch,
    &i_yaw,
    NULL
};

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

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

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

fspec_rv_t core_quat_from_euler_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_from_euler_call_exec(void *dh);

const function_spec_t atomic_core_quat_from_euler_spec = {
    .name = "core.quat.from_euler",
    .annotation = "Covert Euler angles to quaternion",
    .inputs = inputs,
    .outputs = outputs,
    .params = params
};

const function_calls_t atomic_core_quat_from_euler_calls = {
    .interface_handle_size = sizeof(core_quat_from_euler_interface_t),
    .init = NULL,
    .init_inputs = core_quat_from_euler_call_init_inputs,
    .init_outputs = core_quat_from_euler_call_init_outputs,
    .pre_exec_init = NULL,
    .exec = core_quat_from_euler_call_exec,
    .set_params = NULL
};

const function_handler_t atomic_core_quat_from_euler_handler = {
    .spec = &atomic_core_quat_from_euler_spec,
    .calls = &atomic_core_quat_from_euler_calls,
    .extension_handler = NULL
};

core_quat_from_euler_interface.c¶

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

#include "core_quat_from_euler.h"

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

int core_quat_from_euler_interface_inputs_init(
    core_quat_from_euler_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_roll = fspec_find_path2connect(conn_spec,"roll");
    const char *topic_path_in_pitch = fspec_find_path2connect(conn_spec,"pitch");
    const char *topic_path_in_yaw = fspec_find_path2connect(conn_spec,"yaw");

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

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

    // Connecting mandatory input "yaw"
    if (topic_path_in_yaw != NULL) {
        rv = eswb_connect_nested(mounting_td, topic_path_in_yaw, &interface->eswb_descriptors.in_yaw);
        if(rv != eswb_e_ok) {
            error("failed connect input \"yaw\" to topic \"%s\": %s", topic_path_in_yaw, eswb_strerror(rv));
            errcnt_no_topic++;
        }
    } else {
        error("mandatory input \"yaw\" 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_from_euler_interface_inputs_update(core_quat_from_euler_interface_t *interface)
{
    eswb_rv_t rv;

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

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

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

    return 0;
}

fspec_rv_t core_quat_from_euler_interface_outputs_init(
    core_quat_from_euler_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_from_euler_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_from_euler_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_from_euler_interface_outputs_update(core_quat_from_euler_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_from_euler_interface_update(core_quat_from_euler_interface_t *interface)
{
    core_quat_from_euler_interface_inputs_update(interface);
    core_quat_from_euler_exec(&interface->i, &interface->o);
    core_quat_from_euler_interface_outputs_update(interface);
}

fspec_rv_t core_quat_from_euler_call_init_inputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td
)
{
    core_quat_from_euler_interface_t *interface = (core_quat_from_euler_interface_t*) dh;
    return core_quat_from_euler_interface_inputs_init(interface, conn_spec, mounting_td);
}

fspec_rv_t core_quat_from_euler_call_init_outputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
)
{
    core_quat_from_euler_interface_t *interface = (core_quat_from_euler_interface_t*) dh;
    return core_quat_from_euler_interface_outputs_init(interface, conn_spec, mounting_td, func_name);
}

void core_quat_from_euler_call_exec(void *dh)
{
    core_quat_from_euler_interface_t *interface = (core_quat_from_euler_interface_t*) dh;
    core_quat_from_euler_interface_update(interface);
}

← core.quat.euler_correction core.quat.norm →