Skip to content

Commit

Permalink
[ahrs] mlkf: update state interface from wrapper
Browse files Browse the repository at this point in the history
  • Loading branch information
@flixr
flixr committed Mar 10, 2015
1 parent 2af8fd9 commit 0066d9d
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 49 deletions.
45 changes: 0 additions & 45 deletions sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
View file
Expand Up @@ -33,20 +33,13 @@

#include <string.h> /* for memcpy */

#include "state.h"

#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_simple_matrix.h"
#include "generated/airframe.h"

//#include <stdio.h>

#ifndef AHRS_MLKF_OUTPUT_ENABLED
#define AHRS_MLKF_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED)

#ifndef AHRS_MAG_NOISE_X
#define AHRS_MAG_NOISE_X 0.2
#define AHRS_MAG_NOISE_Y 0.2
Expand All @@ -62,7 +55,6 @@ static inline void update_state_heading(const struct FloatVect3 *i_expected,
struct FloatVect3 *b_measured,
struct FloatVect3 *noise);
static inline void reset_state(void);
static inline void set_body_state_from_quat(void);

struct AhrsMlkf ahrs_mlkf;

Expand All @@ -71,7 +63,6 @@ void ahrs_mlkf_init(void)
{

ahrs_mlkf.is_aligned = FALSE;
ahrs_mlkf.output_enabled = AHRS_MLKF_OUTPUT_ENABLED;

/* init ltp_to_imu quaternion as zero/identity rotation */
float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat);
Expand Down Expand Up @@ -121,11 +112,6 @@ bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag);

/* set initial body orientation */
if (ahrs_mlkf.output_enabled) {
set_body_state_from_quat();
}

/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, *lp_gyro);
Expand All @@ -140,9 +126,6 @@ void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt)
{
propagate_ref(gyro, dt);
propagate_state(dt);
if (ahrs_mlkf.output_enabled) {
set_body_state_from_quat();
}
}

void ahrs_mlkf_update_accel(struct Int32Vect3 *accel)
Expand All @@ -157,9 +140,6 @@ void ahrs_mlkf_update_accel(struct Int32Vect3 *accel)
struct FloatVect3 g_noise = {1. + dn, 1. + dn, 1. + dn};
update_state(&earth_g, &imu_g, &g_noise);
reset_state();
if (ahrs_mlkf.output_enabled) {
set_body_state_from_quat();
}
}

void ahrs_mlkf_update_mag(struct Int32Vect3 *mag)
Expand All @@ -169,9 +149,6 @@ void ahrs_mlkf_update_mag(struct Int32Vect3 *mag)
#else
ahrs_mlkf_update_mag_2d(mag);
#endif
if (ahrs_mlkf.output_enabled) {
set_body_state_from_quat();
}
}

void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag)
Expand Down Expand Up @@ -398,25 +375,3 @@ static inline void reset_state(void)
float_quat_identity(&ahrs_mlkf.gibbs_cor);

}

/**
* Compute body orientation and rates from imu orientation and rates
*/
static inline void set_body_state_from_quat(void)
{
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu);

/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat);
/* Set in state interface */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);

/* compute body rates */
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
/* Set state */
stateSetBodyRates_f(&body_rate);

}
1 change: 0 additions & 1 deletion sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
View file
Expand Up @@ -59,7 +59,6 @@ struct AhrsMlkf {

enum AhrsMlkfStatus status;
bool_t is_aligned;
bool_t output_enabled; ///< if TRUE with push the estimation results to the state interface
};

extern struct AhrsMlkf ahrs_mlkf;
Expand Down
48 changes: 45 additions & 3 deletions sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
View file
Expand Up @@ -28,9 +28,19 @@
#include "subsystems/ahrs/ahrs_float_mlkf_wrapper.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#include "state.h"

#ifndef AHRS_MLKF_OUTPUT_ENABLED
#define AHRS_MLKF_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED)

/** if TRUE with push the estimation results to the state interface */
static bool_t ahrs_mlkf_output_enabled;
static uint32_t ahrs_mlkf_last_stamp;

static void set_body_state_from_quat(void);

#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
Expand Down Expand Up @@ -85,6 +95,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
if (last_stamp > 0 && ahrs_mlkf.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_mlkf_propagate(gyro, dt);
set_body_state_from_quat();
}
last_stamp = stamp;
#else
Expand All @@ -93,6 +104,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_mlkf_propagate(gyro, dt);
set_body_state_from_quat();
}
#endif
}
Expand All @@ -103,6 +115,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
{
if (ahrs_mlkf.is_aligned) {
ahrs_mlkf_update_accel(accel);
set_body_state_from_quat();
}
}

Expand All @@ -112,6 +125,7 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)),
{
if (ahrs_mlkf.is_aligned) {
ahrs_mlkf_update_mag(mag);
set_body_state_from_quat();
}
}

Expand All @@ -121,7 +135,10 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
struct Int32Vect3 *lp_mag)
{
if (!ahrs_mlkf.is_aligned) {
ahrs_mlkf_align(lp_gyro, lp_accel, lp_mag);
/* set initial body orientation in state interface if alignment was successful */
if (ahrs_mlkf_align(lp_gyro, lp_accel, lp_mag)) {
set_body_state_from_quat();
}
}
}

Expand All @@ -138,12 +155,36 @@ static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVe

static bool_t ahrs_mlkf_enable_output(bool_t enable)
{
ahrs_mlkf.output_enabled = enable;
return ahrs_mlkf.output_enabled;
ahrs_mlkf_output_enabled = enable;
return ahrs_mlkf_output_enabled;
}

/**
* Compute body orientation and rates from imu orientation and rates
*/
static void set_body_state_from_quat(void)
{
if (ahrs_mlkf_output_enabled) {
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu);

/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat);
/* Set in state interface */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);

/* compute body rates */
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
/* Set state */
stateSetBodyRates_f(&body_rate);
}
}

void ahrs_mlkf_register(void)
{
ahrs_mlkf_output_enabled = AHRS_MLKF_OUTPUT_ENABLED;
ahrs_mlkf_init();
ahrs_register_impl(ahrs_mlkf_enable_output);

Expand All @@ -162,3 +203,4 @@ void ahrs_mlkf_register(void)
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif
}

0 comments on commit 0066d9d

Please sign in to comment.