ahrs_aligner.c File Reference

#include "ahrs_aligner.h"
#include <stdlib.h>
#include "subsystems/imu.h"
#include "led.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/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_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 101 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 105 of file ahrs_aligner.c.

Function Documentation

ahrs_aligner_init()

void ahrs_aligner_init

(

void

)

Definition at line 81 of file ahrs_aligner.c.

References accel_sum, ahrs_aligner, AHRS_ALIGNER_IMU_ID, AHRS_ALIGNER_RUNNING, DefaultPeriodic, gyro_cb(), gyro_ev, gyro_sum, INT_RATES_ZERO, INT_VECT3_ZERO, AhrsAligner::low_noise_cnt, mag_sum, AhrsAligner::noise, register_periodic_telemetry(), samples_idx, send_aligner(), and AhrsAligner::status.

Referenced by ahrs_init(), ins_float_invariant_wrapper_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_run()

void ahrs_aligner_run

(

void

)

Definition at line 108 of file ahrs_aligner.c.

References ABI_BROADCAST, Imu::accel, accel_sum, ahrs_aligner, AHRS_ALIGNER_LOCKED, AHRS_ALIGNER_SAMPLES_NB, get_sys_time_usec(), Imu::gyro, gyro_sum, imu, 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, Imu::mag, mag_sum, AhrsAligner::noise, RATES_ADD, RATES_SDIV, ref_sensor_samples, samples_idx, 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, dev, Imu::gyro, imu, AhrsAligner::low_noise_cnt, AhrsAligner::lp_gyro, AhrsAligner::noise, Int32Rates::p, Int32Rates::q, Int32Rates::r, and AhrsAligner::status.

Referenced by ahrs_aligner_init().

Here is the caller graph for this function:

Variable Documentation

accel_sum

struct Int32Vect3 accel_sum

static

Definition at line 44 of file ahrs_aligner.c.

Referenced by ahrs_aligner_init(), and ahrs_aligner_run().

ahrs_aligner

struct AhrsAligner ahrs_aligner

Definition at line 37 of file ahrs_aligner.c.

Referenced by ahrs_aligner_init(), 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 43 of file ahrs_aligner.c.

Referenced by ahrs_aligner_init(), and ahrs_aligner_run().

mag_sum

struct Int32Vect3 mag_sum

static

Definition at line 45 of file ahrs_aligner.c.

Referenced by ahrs_aligner_init(), 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_init(), and ahrs_aligner_run().