Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_extern_quat.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Michal Podhradsky
3  * Utah State University, http://aggieair.usu.edu/
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
32 #include "ahrs_extern_quat.h"
33 #include "mcu_periph/sys_time.h"
34 #include "led.h"
35 
37 
38 void ahrs_init(void) {
40 
41  /* set ltp_to_imu so that body is zero */
44 
45  #ifdef IMU_MAG_OFFSET
46  ahrs_impl.mag_offset = IMU_MAG_OFFSET;
47  #else
48  ahrs_impl.mag_offset = 0.;
49  #endif
50 
51  //Needed to set orientations
53 
54  #ifdef AHRS_ALIGNER_LED
55  LED_ON(AHRS_ALIGNER_LED);
56  #endif
57 }
58 
59 void ahrs_propagate(void) {
60  /* Compute LTP to BODY quaternion */
61  struct Int32Quat ltp_to_body_quat;
63  stateSetNedToBodyQuat_i(&ltp_to_body_quat);
64 
65  // TODO: compensate for magnetic offset
66 
67  struct Int32Rates body_rate;
69  /* compute body rates */
71  /* Set state */
72  stateSetBodyRates_i(&body_rate);
73 }
74 
75 void ahrs_align(void) {
76 }
77 
78 void ahrs_update_accel(void) {
79 }
80 
81 void ahrs_update_mag(void) {
82 }
83 
84 void ahrs_update_gps(void) {
85 }
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
Rotation quaternion.
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
#define LED_ON(i)
Definition: led_hw.h:28
#define INT_RATES_ZERO(_e)
struct AhrsIntExternQuat ahrs_impl
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)
Architecture independent timing functions.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
void ahrs_init(void)
AHRS initialization.
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1055
angular rates
static void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition: state.h:970
#define AHRS_UNINIT
Definition: ahrs.h:35
struct Int32Quat body_to_imu_quat
rotation from body to imu frame as a unit quaternion
Definition: imu.h:51
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:476
struct Int32Quat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as quaternions.
AHRS interface for multiple IMU/AHRS subsystems, such as GX3, UM6 etc.
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
arch independent LED (Light Emitting Diodes) API
void ahrs_align(void)
Aligns the AHRS.
struct Int32Rates imu_rate
Rotational velocity in IMU frame.
#define AHRS_RUNNING
Definition: ahrs.h:36
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
void ahrs_propagate(void)
Propagation.