core.nav.att_from_accel¶

Inputs¶

Name	Type	Title	Mandatory	Description
a	<fspeclib.Structure object at 0x7f2b33a7cdf0>	Input acceleration vector	True	None

Outputs¶

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

Parameters¶

Function has no parameters

State variables¶

Function has no state variables

Usage XML code snippet¶

core_nav_att_from_accel snippet for FLOW configuration file¶

<f name="att_from_accel" by_spec="core.nav.att_from_accel">
    <in alias="a">some_block_1/output</in>
</f>

Function’s artifacts¶

Declaration for core_nav_att_from_accel
Implementation for core_nav_att_from_accel
Header of core_nav_att_from_accel
Specification of core_nav_att_from_accel
Interface of core_nav_att_from_accel

declaration.py¶

from fspeclib import *


Function(
    name='core.nav.att_from_accel',
    title=LocalizedString(
        en='Calculate attitude based on accelerometers values'
    ),

    inputs=[
        Input(
            name='a',
            title='Input acceleration vector',
            value_type='core.type.v3f64'
        ),
    ],

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

core_nav_att_from_accel_exec.c¶

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

#define square(__a) ((__a)*(__a))

void core_nav_att_from_accel_exec(const core_nav_att_from_accel_inputs_t *i, core_nav_att_from_accel_outputs_t *o)
{
    o->roll = atan2(i->a.y, i->a.z);
    o->pitch = atan2(-i->a.x, sqrt(square(i->a.z) + square(i->a.y)));
}

core_nav_att_from_accel.h¶

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

#ifndef FSPEC_CORE_NAV_ATT_FROM_ACCEL_H
#define FSPEC_CORE_NAV_ATT_FROM_ACCEL_H

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

#include "function.h"

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

/**
 * @brief Inputs of `core.nav.att_from_accel` function
 */
typedef struct core_nav_att_from_accel_inputs_ {
    core_type_v3f64_t a;  /// Input acceleration vector

} core_nav_att_from_accel_inputs_t;

/**
 * @brief Outputs of `core.nav.att_from_accel` function
 */
typedef struct core_nav_att_from_accel_outputs_ {
    core_type_f64_t roll; /// Roll

    core_type_f64_t pitch; /// Pitch

} core_nav_att_from_accel_outputs_t;

typedef struct core_nav_att_from_accel_eswb_descriptors_ {
    eswb_topic_descr_t in_a;
    eswb_topic_descr_t out_all;
} core_nav_att_from_accel_eswb_descriptors_t;

typedef struct core_nav_att_from_accel_interface_ {
    core_nav_att_from_accel_inputs_t i;
    core_nav_att_from_accel_outputs_t o;
    core_nav_att_from_accel_eswb_descriptors_t eswb_descriptors;
} core_nav_att_from_accel_interface_t;

void core_nav_att_from_accel_exec(const core_nav_att_from_accel_inputs_t *i, core_nav_att_from_accel_outputs_t *o);

#endif // FSPEC_CORE_NAV_ATT_FROM_ACCEL_H

core_nav_att_from_accel_spec.c¶

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

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

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

static const input_spec_t i_a = {
    .name = "a",
    .annotation = "Input acceleration vector",
    .flags = 0
};

static const input_spec_t *inputs[2] = {
    &i_a,
    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 *outputs[3] = {
    &o_roll,
    &o_pitch,
    NULL
};

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

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

void core_nav_att_from_accel_call_exec(void *dh);

const function_spec_t atomic_core_nav_att_from_accel_spec = {
    .name = "core.nav.att_from_accel",
    .annotation = "Calculate attitude based on accelerometers values",
    .inputs = inputs,
    .outputs = outputs,
    .params = params
};

const function_calls_t atomic_core_nav_att_from_accel_calls = {
    .interface_handle_size = sizeof(core_nav_att_from_accel_interface_t),
    .init = NULL,
    .init_inputs = core_nav_att_from_accel_call_init_inputs,
    .init_outputs = core_nav_att_from_accel_call_init_outputs,
    .pre_exec_init = NULL,
    .exec = core_nav_att_from_accel_call_exec,
    .set_params = NULL
};

const function_handler_t atomic_core_nav_att_from_accel_handler = {
    .spec = &atomic_core_nav_att_from_accel_spec,
    .calls = &atomic_core_nav_att_from_accel_calls,
    .extension_handler = NULL
};

core_nav_att_from_accel_interface.c¶

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

#include "core_nav_att_from_accel.h"

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

int core_nav_att_from_accel_interface_inputs_init(
    core_nav_att_from_accel_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_a = fspec_find_path2connect(conn_spec,"a");

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

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

    return 0;
}

fspec_rv_t core_nav_att_from_accel_interface_outputs_init(
    core_nav_att_from_accel_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, 3);
    core_nav_att_from_accel_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_nav_att_from_accel_outputs_t, roll, "roll", tt_double);
    usr_topic_add_struct_child(cntx, rt, core_nav_att_from_accel_outputs_t, pitch, "pitch", 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_nav_att_from_accel_interface_outputs_update(core_nav_att_from_accel_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_nav_att_from_accel_interface_update(core_nav_att_from_accel_interface_t *interface)
{
    core_nav_att_from_accel_interface_inputs_update(interface);
    core_nav_att_from_accel_exec(&interface->i, &interface->o);
    core_nav_att_from_accel_interface_outputs_update(interface);
}

fspec_rv_t core_nav_att_from_accel_call_init_inputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td
)
{
    core_nav_att_from_accel_interface_t *interface = (core_nav_att_from_accel_interface_t*) dh;
    return core_nav_att_from_accel_interface_inputs_init(interface, conn_spec, mounting_td);
}

fspec_rv_t core_nav_att_from_accel_call_init_outputs(
    void *dh,
    const func_conn_spec_t *conn_spec,
    eswb_topic_descr_t mounting_td,
    const char *func_name
)
{
    core_nav_att_from_accel_interface_t *interface = (core_nav_att_from_accel_interface_t*) dh;
    return core_nav_att_from_accel_interface_outputs_init(interface, conn_spec, mounting_td, func_name);
}

void core_nav_att_from_accel_call_exec(void *dh)
{
    core_nav_att_from_accel_interface_t *interface = (core_nav_att_from_accel_interface_t*) dh;
    core_nav_att_from_accel_interface_update(interface);
}

← core.math.vector3f64 core.quat.euler_correction →