core.quat.to_euler¶

Inputs¶

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

Outputs¶

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

Parameters¶

Function has no parameters

State variables¶

Function has no state variables

Usage XML code snippet¶

core_quat_to_euler snippet for FLOW configuration file¶

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

Function’s artifacts¶

Declaration for core_quat_to_euler
Implementation for core_quat_to_euler
Header of core_quat_to_euler
Specification of core_quat_to_euler
Interface of core_quat_to_euler

declaration.py¶

from fspeclib import *


Function(
    name='core.quat.to_euler',
    title=LocalizedString(
        en='Covert quaternion to Euler angles'
    ),

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

    outputs=[
        Output(
            name='roll',
            title='Roll',
            value_type='core.type.f64'
        ),
        Output(
            name='pitch',
            title='Pitch',
            value_type='core.type.f64'
        ),
        Output(
            name='yaw',
            title='Yaw',
            value_type='core.type.f64'
        ),
    ],
)

core_quat_to_euler_exec.c¶

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

#define cmp(a,b,sign) ((a) sign (b)) ? (a) : (b)

#define min(a,b) ((a) < (b)) ? (a) : (b)
#define max(a,b) ((a) > (b)) ? (a) : (b)

#define rad2deg(r)  ((r) * 180.0 / M_PI)

void core_quat_to_euler_exec(const core_quat_to_euler_inputs_t *i, core_quat_to_euler_outputs_t *o)
{
#define square(s) ((s)*(s))
    core_type_f64_t asin_arg = 2*( i->q.w * i->q.y - i->q.z * i->q.x);
    core_type_f64_t q2_sq = square (i->q.y);

    asin_arg = max ( -1.0, asin_arg );
    asin_arg = min (  1.0, asin_arg );

    o->roll = atan2( 2 * (i->q.w * i->q.x + i->q.y*i->q.z), 1 - 2 * ( square ( i->q.x) + q2_sq ) );
    o->pitch = asin( asin_arg );
    o->yaw = atan2( 2 * (i->q.w * i->q.z + i->q.x * i->q.y), 1 - 2 * ( q2_sq + square (i->q.z) ) );

    // printf("R=%.2f P=%.2f Y=%.2f\n", rad2deg(o->roll), rad2deg(o->pitch), rad2deg(o->yaw));
}

core_quat_to_euler.h¶

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

#ifndef FSPEC_CORE_QUAT_TO_EULER_H
#define FSPEC_CORE_QUAT_TO_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.to_euler` function
 */
typedef struct core_quat_to_euler_inputs_ {
    core_type_quat_t q;  /// Input quat

} core_quat_to_euler_inputs_t;

/**
 * @brief Outputs of `core.quat.to_euler` function
 */
typedef struct core_quat_to_euler_outputs_ {
    core_type_f64_t roll; /// Roll

    core_type_f64_t pitch; /// Pitch

    core_type_f64_t yaw; /// Yaw

} core_quat_to_euler_outputs_t;

typedef struct core_quat_to_euler_eswb_descriptors_ {
    eswb_topic_descr_t in_q;
    eswb_topic_descr_t out_all;
} core_quat_to_euler_eswb_descriptors_t;

typedef struct core_quat_to_euler_interface_ {
    core_quat_to_euler_inputs_t i;
    core_quat_to_euler_outputs_t o;
    core_quat_to_euler_eswb_descriptors_t eswb_descriptors;
} core_quat_to_euler_interface_t;

void core_quat_to_euler_exec(const core_quat_to_euler_inputs_t *i, core_quat_to_euler_outputs_t *o);

#endif // FSPEC_CORE_QUAT_TO_EULER_H

core_quat_to_euler_spec.c¶

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

#include "core_quat_to_euler.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_roll = {
    .name = "roll",
    .annotation = "Roll",
    .flags = 0
};

static const output_spec_t o_pitch = {
    .name = "pitch",
    .annotation = "Pitch",
    .flags = 0
};

static const output_spec_t o_yaw = {
    .name = "yaw",
    .annotation = "Yaw",
    .flags = 0
};

static const output_spec_t *outputs[4] = {
    &o_roll,
    &o_pitch,
    &o_yaw,
    NULL
};

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

fspec_rv_t core_quat_to_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_to_euler_call_exec(void *dh);

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

const function_calls_t atomic_core_quat_to_euler_calls = {
    .interface_handle_size = sizeof(core_quat_to_euler_interface_t),
    .init = NULL,
    .init_inputs = core_quat_to_euler_call_init_inputs,
    .init_outputs = core_quat_to_euler_call_init_outputs,
    .pre_exec_init = NULL,
    .exec = core_quat_to_euler_call_exec,
    .set_params = NULL
};

const function_handler_t atomic_core_quat_to_euler_handler = {
    .spec = &atomic_core_quat_to_euler_spec,
    .calls = &atomic_core_quat_to_euler_calls,
    .extension_handler = NULL
};

core_quat_to_euler_interface.c¶

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

#include "core_quat_to_euler.h"

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

int core_quat_to_euler_interface_inputs_init(
    core_quat_to_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_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_to_euler_interface_inputs_update(core_quat_to_euler_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_to_euler_interface_outputs_init(
    core_quat_to_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, 4);
    core_quat_to_euler_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_quat_to_euler_outputs_t, roll, "roll", tt_double);
    usr_topic_add_struct_child(cntx, rt, core_quat_to_euler_outputs_t, pitch, "pitch", tt_double);
    usr_topic_add_struct_child(cntx, rt, core_quat_to_euler_outputs_t, yaw, "yaw", 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_to_euler_interface_outputs_update(core_quat_to_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_to_euler_interface_update(core_quat_to_euler_interface_t *interface)
{
    core_quat_to_euler_interface_inputs_update(interface);
    core_quat_to_euler_exec(&interface->i, &interface->o);
    core_quat_to_euler_interface_outputs_update(interface);
}

fspec_rv_t core_quat_to_euler_call_init_inputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td
)
{
    core_quat_to_euler_interface_t *interface = (core_quat_to_euler_interface_t*) dh;
    return core_quat_to_euler_interface_inputs_init(interface, conn_spec, mounting_td);
}

fspec_rv_t core_quat_to_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_to_euler_interface_t *interface = (core_quat_to_euler_interface_t*) dh;
    return core_quat_to_euler_interface_outputs_init(interface, conn_spec, mounting_td, func_name);
}

void core_quat_to_euler_call_exec(void *dh)
{
    core_quat_to_euler_interface_t *interface = (core_quat_to_euler_interface_t*) dh;
    core_quat_to_euler_interface_update(interface);
}

← core.quat.prop core.source.constant →