ahrs_aligner.c File Reference

Low-pass IMU measurements at startup to align the AHRS. More...

#include "ahrs_aligner.h"
#include <stdlib.h>
#include "modules/imu/imu.h"
#include "led.h"
#include "modules/core/abi.h"
#include "mcu_periph/sys_time.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for ahrs_aligner.c:

Go to the source code of this file.

Macros
#define	AHRS_ALIGNER_SAMPLES_NB   100
#define	AHRS_ALIGNER_IMU_ID   ABI_BROADCAST
#define	LOW_NOISE_THRESHOLD   90000
#define	LOW_NOISE_TIME   5
	Number of cycles (100 samples each) with low noise. More...

Functions
static void	gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void	send_aligner (struct transport_tx *trans, struct link_device *dev)
void	ahrs_aligner_init (void)
void	ahrs_aligner_restart (void)
void	ahrs_aligner_run (void)

Variables
struct AhrsAligner	ahrs_aligner
static struct Int32Rates	gyro_sum
static struct Int32Vect3	accel_sum
static struct Int32Vect3	mag_sum
static int32_t	ref_sensor_samples [AHRS_ALIGNER_SAMPLES_NB]
static uint32_t	samples_idx
static abi_event	gyro_ev

Detailed Description

Low-pass IMU measurements at startup to align the AHRS.

Definition in file ahrs_aligner.c.

Macro Definition Documentation

AHRS_ALIGNER_IMU_ID

#define AHRS_ALIGNER_IMU_ID   ABI_BROADCAST

Definition at line 50 of file ahrs_aligner.c.

AHRS_ALIGNER_SAMPLES_NB

#define AHRS_ALIGNER_SAMPLES_NB   100

Definition at line 40 of file ahrs_aligner.c.

LOW_NOISE_THRESHOLD

#define LOW_NOISE_THRESHOLD   90000

Definition at line 108 of file ahrs_aligner.c.

LOW_NOISE_TIME

#define LOW_NOISE_TIME   5

Number of cycles (100 samples each) with low noise.

Definition at line 112 of file ahrs_aligner.c.

Function Documentation

ahrs_aligner_init()

void ahrs_aligner_init

(

void

)

Definition at line 84 of file ahrs_aligner.c.

References AHRS_ALIGNER_IMU_ID, ahrs_aligner_restart(), DefaultPeriodic, gyro_cb(), gyro_ev, register_periodic_telemetry(), and send_aligner().

Referenced by ahrs_init(), ins_float_invariant_wrapper_init(), ins_flow_init(), and ins_mekf_wind_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_aligner_restart()

void ahrs_aligner_restart

(

void

)

Definition at line 96 of file ahrs_aligner.c.

References accel_sum, ahrs_aligner, AHRS_ALIGNER_RUNNING, gyro_sum, INT_RATES_ZERO, INT_VECT3_ZERO, AhrsAligner::low_noise_cnt, mag_sum, AhrsAligner::noise, samples_idx, and AhrsAligner::status.

Referenced by ahrs_aligner_init(), and gyro_cb().

Here is the caller graph for this function:

ahrs_aligner_run()

void ahrs_aligner_run

(

void

)

Definition at line 115 of file ahrs_aligner.c.

References accel_sum, ahrs_aligner, AHRS_ALIGNER_ID, AHRS_ALIGNER_IMU_ID, AHRS_ALIGNER_LOCKED, AHRS_ALIGNER_SAMPLES_NB, get_sys_time_usec(), gyro_sum, imu_get_accel(), imu_get_gyro(), imu_get_mag(), INT_RATES_ZERO, INT_VECT3_ZERO, LED_ON, LED_TOGGLE, AhrsAligner::low_noise_cnt, LOW_NOISE_THRESHOLD, LOW_NOISE_TIME, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, AhrsAligner::lp_mag, mag_sum, AhrsAligner::noise, RATES_ADD, RATES_SDIV, ref_sensor_samples, samples_idx, imu_gyro_t::scaled, imu_accel_t::scaled, imu_mag_t::scaled, AhrsAligner::status, VECT3_ADD, VECT3_SDIV, and Int32Vect3::z.

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

gyro_cb()

static void gyro_cb ( uint8_t  sender_id, uint32_t  stamp, struct Int32Rates *  gyro  )

static

Definition at line 54 of file ahrs_aligner.c.

References ahrs_aligner, AHRS_ALIGNER_LOCKED, ahrs_aligner_run(), and AhrsAligner::status.

Referenced by ahrs_aligner_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_aligner()

static void send_aligner ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 66 of file ahrs_aligner.c.

References ahrs_aligner, AHRS_ALIGNER_IMU_ID, dev, imu_get_gyro(), AhrsAligner::low_noise_cnt, AhrsAligner::lp_gyro, AhrsAligner::noise, Int32Rates::p, Int32Rates::q, Int32Rates::r, imu_gyro_t::scaled, and AhrsAligner::status.

Referenced by ahrs_aligner_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

accel_sum

struct Int32Vect3 accel_sum

static

Definition at line 1 of file ahrs_aligner.c.

Referenced by ahrs_aligner_restart(), and ahrs_aligner_run().

ahrs_aligner

struct AhrsAligner ahrs_aligner

Definition at line 1 of file ahrs_aligner.c.

Referenced by ahrs_aligner_restart(), ahrs_aligner_run(), gyro_cb(), and send_aligner().

gyro_ev

abi_event gyro_ev

static

Definition at line 52 of file ahrs_aligner.c.

Referenced by ahrs_aligner_init().

gyro_sum

struct Int32Rates gyro_sum

static

Definition at line 1 of file ahrs_aligner.c.

Referenced by ahrs_aligner_restart(), and ahrs_aligner_run().

mag_sum

struct Int32Vect3 mag_sum

static

Definition at line 1 of file ahrs_aligner.c.

Referenced by ahrs_aligner_restart(), and ahrs_aligner_run().

ref_sensor_samples

int32_t ref_sensor_samples[AHRS_ALIGNER_SAMPLES_NB]

static

Definition at line 46 of file ahrs_aligner.c.

Referenced by ahrs_aligner_run().

samples_idx

uint32_t samples_idx

static

Definition at line 47 of file ahrs_aligner.c.

Referenced by ahrs_aligner_restart(), and ahrs_aligner_run().