core.quat.euler_correction¶

Inputs¶

Name	Type	Title	Mandatory	Description
roll_err	core.type.f64	Roll	True	None
pitch_err	core.type.f64	Pitch	True	None
yaw_err	core.type.f64	Yaw	True	None
q	<fspeclib.Structure object at 0x7f2b33a7c250>	Quaternion to correct	True	None
roll	core.type.f64	Current roll	True	None

Outputs¶

Name	Type	Title	Description
q	<fspeclib.Structure object at 0x7f2b33a7c250>	Correction quaternion	None

Parameters¶

Function has no parameters

State variables¶

Function has no state variables

Usage XML code snippet¶

core_quat_euler_correction snippet for FLOW configuration file¶

<f name="euler_correction" by_spec="core.quat.euler_correction">
    <in alias="roll_err">some_block_1/output</in>
    <in alias="pitch_err">some_block_2/output</in>
    <in alias="yaw_err">some_block_3/output</in>
    <in alias="q">some_block_4/output</in>
    <in alias="roll">some_block_5/output</in>
</f>

Function’s artifacts¶

Declaration for core_quat_euler_correction
Implementation for core_quat_euler_correction
Header of core_quat_euler_correction
Specification of core_quat_euler_correction
Interface of core_quat_euler_correction

declaration.py¶

from fspeclib import *





Function(
    name='core.quat.euler_correction',
    title=LocalizedString(
        en='Calculate correction quaternion from Euler angles errors'
    ),
    inputs=[
        Input(
            name='roll_err',
            title='Roll',
            value_type='core.type.f64'
        ),
        Input(
            name='pitch_err',
            title='Pitch',
            value_type='core.type.f64'
        ),
        Input(
            name='yaw_err',
            title='Yaw',
            value_type='core.type.f64'
        ),

        Input(
            name='q',
            title='Quaternion to correct',
            value_type='core.type.quat'
        ),

        Input(
            name='roll',
            title='Current roll',
            value_type='core.type.f64'
        ),

    ],
    outputs=[
        Output(
            name='q',
            title='Correction quaternion',
            value_type='core.type.quat'
        ),
    ],
)

core_quat_euler_correction_exec.c¶

#include <math.h>

#include "core_quat_euler_correction.h"


void quat_product ( const core_type_quat_t * ql, const core_type_quat_t* qr, core_type_quat_t* out ) {
    core_type_quat_t tmp;
    tmp.w = qr->w * ql->w - qr->x * ql->x - qr->y * ql->y - qr->z * ql->z;
    tmp.x = qr->w * ql->x + qr->x * ql->w - qr->y * ql->z + qr->z * ql->y;
    tmp.y = qr->w * ql->y + qr->x * ql->z + qr->y * ql->w - qr->z * ql->x;
    tmp.z = qr->w * ql->z - qr->x * ql->y + qr->y * ql->x + qr->z * ql->w;
    *out = tmp;
}

void core_quat_euler_correction_exec(
    const core_quat_euler_correction_inputs_t *i,
    core_quat_euler_correction_outputs_t *o
)
{
    core_type_quat_t q_err;
    core_type_quat_t q_out;

    if (fabs(i->yaw_err) > 0.0) {
        q_err.w = cos(-i->yaw_err * 0.5);
        q_err.x = 0;
        q_err.y = 0;
        q_err.z = sin(-i->yaw_err * 0.5);

        quat_product(&q_err, &i->q, &q_out);
    } else {
        q_out = i->q;
    }

    if ((fabs(i->roll_err) > 0.0) || (fabs(i->pitch_err) > 0.0)) {
        q_err.w = cos(-i->roll_err * 0.5);
        q_err.x = sin(-i->roll_err * 0.5);;
        q_err.y = 0;
        q_err.z = 0;

        core_type_quat_t q_err2;
        core_type_f64_t sin_err_pitch = sin(-i->pitch_err * 0.5);
        q_err2.w = cos(-i->pitch_err * 0.5);;
        q_err2.x = 0;
        q_err2.y =  sin_err_pitch * cos(-i->roll);
        q_err2.z = -sin_err_pitch * sin(-i->roll);

        quat_product(&q_err2, &q_err, &q_err);
        quat_product(&q_out, &q_err, &q_out);
    }

    o->q = q_out;
}

core_quat_euler_correction.h¶

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

#ifndef FSPEC_CORE_QUAT_EULER_CORRECTION_H
#define FSPEC_CORE_QUAT_EULER_CORRECTION_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.euler_correction` function
 */
typedef struct core_quat_euler_correction_inputs_ {
    core_type_f64_t roll_err;  /// Roll
    core_type_f64_t pitch_err;  /// Pitch
    core_type_f64_t yaw_err;  /// Yaw
    core_type_quat_t q;  /// Quaternion to correct
    core_type_f64_t roll;  /// Current roll

} core_quat_euler_correction_inputs_t;

/**
 * @brief Outputs of `core.quat.euler_correction` function
 */
typedef struct core_quat_euler_correction_outputs_ {
    core_type_quat_t q; /// Correction quaternion

} core_quat_euler_correction_outputs_t;

typedef struct core_quat_euler_correction_eswb_descriptors_ {
    eswb_topic_descr_t in_roll_err;
    eswb_topic_descr_t in_pitch_err;
    eswb_topic_descr_t in_yaw_err;
    eswb_topic_descr_t in_q;
    eswb_topic_descr_t in_roll;
    eswb_topic_descr_t out_all;
} core_quat_euler_correction_eswb_descriptors_t;

typedef struct core_quat_euler_correction_interface_ {
    core_quat_euler_correction_inputs_t i;
    core_quat_euler_correction_outputs_t o;
    core_quat_euler_correction_eswb_descriptors_t eswb_descriptors;
} core_quat_euler_correction_interface_t;

void core_quat_euler_correction_exec(
    const core_quat_euler_correction_inputs_t *i,
    core_quat_euler_correction_outputs_t *o
);

#endif // FSPEC_CORE_QUAT_EULER_CORRECTION_H

core_quat_euler_correction_spec.c¶

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

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

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

static const input_spec_t i_roll_err = {
    .name = "roll_err",
    .annotation = "Roll",
    .flags = 0
};

static const input_spec_t i_pitch_err = {
    .name = "pitch_err",
    .annotation = "Pitch",
    .flags = 0
};

static const input_spec_t i_yaw_err = {
    .name = "yaw_err",
    .annotation = "Yaw",
    .flags = 0
};

static const input_spec_t i_q = {
    .name = "q",
    .annotation = "Quaternion to correct",
    .flags = 0
};

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

static const input_spec_t *inputs[6] = {
    &i_roll_err,
    &i_pitch_err,
    &i_yaw_err,
    &i_q,
    &i_roll,
    NULL
};

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

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

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

fspec_rv_t core_quat_euler_correction_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_euler_correction_call_exec(void *dh);

const function_spec_t atomic_core_quat_euler_correction_spec = {
    .name = "core.quat.euler_correction",
    .annotation = "Calculate correction quaternion from Euler angles errors",
    .inputs = inputs,
    .outputs = outputs,
    .params = params
};

const function_calls_t atomic_core_quat_euler_correction_calls = {
    .interface_handle_size = sizeof(core_quat_euler_correction_interface_t),
    .init = NULL,
    .init_inputs = core_quat_euler_correction_call_init_inputs,
    .init_outputs = core_quat_euler_correction_call_init_outputs,
    .pre_exec_init = NULL,
    .exec = core_quat_euler_correction_call_exec,
    .set_params = NULL
};

const function_handler_t atomic_core_quat_euler_correction_handler = {
    .spec = &atomic_core_quat_euler_correction_spec,
    .calls = &atomic_core_quat_euler_correction_calls,
    .extension_handler = NULL
};

core_quat_euler_correction_interface.c¶

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

#include "core_quat_euler_correction.h"

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

int core_quat_euler_correction_interface_inputs_init(
    core_quat_euler_correction_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_err = fspec_find_path2connect(conn_spec,"roll_err");
    const char *topic_path_in_pitch_err = fspec_find_path2connect(conn_spec,"pitch_err");
    const char *topic_path_in_yaw_err = fspec_find_path2connect(conn_spec,"yaw_err");
    const char *topic_path_in_q = fspec_find_path2connect(conn_spec,"q");
    const char *topic_path_in_roll = fspec_find_path2connect(conn_spec,"roll");

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

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

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

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

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

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

    rv = eswb_read(interface->eswb_descriptors.in_yaw_err, &interface->i.yaw_err);
    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*/
    }

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

    return 0;
}

fspec_rv_t core_quat_euler_correction_interface_outputs_init(
    core_quat_euler_correction_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_euler_correction_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_euler_correction_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_euler_correction_interface_outputs_update(core_quat_euler_correction_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_euler_correction_interface_update(core_quat_euler_correction_interface_t *interface)
{
    core_quat_euler_correction_interface_inputs_update(interface);
    core_quat_euler_correction_exec(&interface->i, &interface->o);
    core_quat_euler_correction_interface_outputs_update(interface);
}

fspec_rv_t core_quat_euler_correction_call_init_inputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td
)
{
    core_quat_euler_correction_interface_t *interface = (core_quat_euler_correction_interface_t*) dh;
    return core_quat_euler_correction_interface_inputs_init(interface, conn_spec, mounting_td);
}

fspec_rv_t core_quat_euler_correction_call_init_outputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
)
{
    core_quat_euler_correction_interface_t *interface = (core_quat_euler_correction_interface_t*) dh;
    return core_quat_euler_correction_interface_outputs_init(interface, conn_spec, mounting_td, func_name);
}

void core_quat_euler_correction_call_exec(void *dh)
{
    core_quat_euler_correction_interface_t *interface = (core_quat_euler_correction_interface_t*) dh;
    core_quat_euler_correction_interface_update(interface);
}

← core.nav.att_from_accel core.quat.from_euler →