Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ahrs_aligner.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21
28
29#include "ahrs_aligner.h"
30
31#include <stdlib.h> /* for abs() */
32#include "modules/imu/imu.h"
33#include "led.h"
34#include "modules/core/abi.h"
35#include "mcu_periph/sys_time.h"
36
38
39#ifndef AHRS_ALIGNER_SAMPLES_NB
40#define AHRS_ALIGNER_SAMPLES_NB 100
41#endif
42
43static struct Int32Rates gyro_sum;
44static struct Int32Vect3 accel_sum;
45static struct Int32Vect3 mag_sum;
48
49#ifndef AHRS_ALIGNER_IMU_ID
50#define AHRS_ALIGNER_IMU_ID ABI_BROADCAST
51#endif
53
54static void gyro_cb(uint8_t sender_id __attribute__((unused)),
55 uint32_t stamp __attribute__((unused)),
56 struct Int32Rates *gyro __attribute__((unused)))
57{
58 if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
60 }
61}
62
63#if PERIODIC_TELEMETRY
65
66static void send_aligner(struct transport_tx *trans, struct link_device *dev)
67{
68 struct imu_gyro_t *gyro = imu_get_gyro(AHRS_ALIGNER_IMU_ID, false);
69 if(gyro != NULL) {
70 pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID,
71 &ahrs_aligner.lp_gyro.p,
72 &ahrs_aligner.lp_gyro.q,
73 &ahrs_aligner.lp_gyro.r,
74 &gyro->scaled.p,
75 &gyro->scaled.q,
76 &gyro->scaled.r,
77 &ahrs_aligner.noise,
78 &ahrs_aligner.low_noise_cnt,
79 &ahrs_aligner.status);
80 }
81}
82#endif
83
85{
87
88 // for now: only bind to gyro message and still read from global imu struct
89 AbiBindMsgIMU_GYRO(AHRS_ALIGNER_IMU_ID, &gyro_ev, gyro_cb);
90
91#if PERIODIC_TELEMETRY
92 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FILTER_ALIGNER, send_aligner);
93#endif
94}
95
97{
102 samples_idx = 0;
103 ahrs_aligner.noise = 0;
104 ahrs_aligner.low_noise_cnt = 0;
105}
106
107#ifndef LOW_NOISE_THRESHOLD
108#define LOW_NOISE_THRESHOLD 90000
109#endif
111#ifndef LOW_NOISE_TIME
112#define LOW_NOISE_TIME 5
113#endif
114
116{
117 struct imu_gyro_t *gyro = imu_get_gyro(AHRS_ALIGNER_IMU_ID, false);
118 struct imu_accel_t *accel = imu_get_accel(AHRS_ALIGNER_IMU_ID, false);
119 struct imu_mag_t *mag = imu_get_mag(AHRS_ALIGNER_IMU_ID, false);
120
121 // Could not find all sensors
122 if(gyro == NULL || accel == NULL)
123 return;
124
125 RATES_ADD(gyro_sum, gyro->scaled);
126 VECT3_ADD(accel_sum, accel->scaled);
127 if(mag != NULL)
128 VECT3_ADD(mag_sum, mag->scaled);
129
131 samples_idx++;
132
133#ifdef AHRS_ALIGNER_LED
134 RunOnceEvery(50, {LED_TOGGLE(AHRS_ALIGNER_LED);});
135#endif
136
138 int32_t avg_ref_sensor = accel_sum.z;
139 if (avg_ref_sensor >= 0) {
140 avg_ref_sensor += AHRS_ALIGNER_SAMPLES_NB / 2;
141 } else {
142 avg_ref_sensor -= AHRS_ALIGNER_SAMPLES_NB / 2;
143 }
144 avg_ref_sensor /= AHRS_ALIGNER_SAMPLES_NB;
145
146 ahrs_aligner.noise = 0;
147 int i;
148 for (i = 0; i < AHRS_ALIGNER_SAMPLES_NB; i++) {
149 int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
150 ahrs_aligner.noise += abs(diff);
151 }
152
156
160 samples_idx = 0;
161
162 if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD) {
163 ahrs_aligner.low_noise_cnt++;
164 } else if (ahrs_aligner.low_noise_cnt > 0) {
165 ahrs_aligner.low_noise_cnt--;
166 }
167
168 if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) {
170#ifdef AHRS_ALIGNER_LED
171 LED_ON(AHRS_ALIGNER_LED);
172#endif
173 uint32_t now_ts = get_sys_time_usec();
174 AbiSendMsgIMU_LOWPASSED(AHRS_ALIGNER_ID, now_ts, &ahrs_aligner.lp_gyro,
175 &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
176 }
177 }
178
179}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define AHRS_ALIGNER_ID
static int32_t ref_sensor_samples[AHRS_ALIGNER_SAMPLES_NB]
#define AHRS_ALIGNER_SAMPLES_NB
static struct Int32Vect3 mag_sum
void ahrs_aligner_init(void)
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static uint32_t samples_idx
struct AhrsAligner ahrs_aligner
static struct Int32Vect3 accel_sum
static abi_event gyro_ev
void ahrs_aligner_restart(void)
void ahrs_aligner_run(void)
static void send_aligner(struct transport_tx *trans, struct link_device *dev)
#define LOW_NOISE_THRESHOLD
#define AHRS_ALIGNER_IMU_ID
static struct Int32Rates gyro_sum
#define LOW_NOISE_TIME
Number of cycles (100 samples each) with low noise.
Interface to align the AHRS via low-passed measurements at startup.
#define AHRS_ALIGNER_LOCKED
#define AHRS_ALIGNER_RUNNING
#define LED_ON(i)
Definition led_hw.h:51
#define LED_TOGGLE(i)
Definition led_hw.h:53
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
#define VECT3_SDIV(_vo, _vi, _s)
#define RATES_ADD(_a, _b)
#define VECT3_ADD(_a, _b)
#define RATES_SDIV(_ro, _ri, _s)
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
#define INT_RATES_ZERO(_e)
#define INT_VECT3_ZERO(_v)
angular rates
struct imu_gyro_t * imu_get_gyro(uint8_t sender_id, bool create)
Find or create the gyro in the imu structure.
Definition imu.c:1170
struct imu_accel_t * imu_get_accel(uint8_t sender_id, bool create)
Find or create the accel in the imu structure.
Definition imu.c:1193
struct imu_mag_t * imu_get_mag(uint8_t sender_id, bool create)
Find or create the mag in the imu structure.
Definition imu.c:1217
Inertial Measurement Unit interface.
struct Int32Rates scaled
Last scaled values in body frame.
Definition imu.h:68
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition imu.h:88
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition imu.h:106
arch independent LED (Light Emitting Diodes) API
Architecture independent timing functions.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.