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 →