Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ahrs_infrared.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 The Paparazzi Team
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 
23 
25 #include "subsystems/imu.h"
26 #include "subsystems/gps.h"
27 
28 #include "estimator.h"
29 
30 
31 
32 void ahrs_init(void) {
34 
35  /* set ltp_to_body to zero */
40 
41  /* set ltp_to_body to zero */
46 }
47 
48 void ahrs_align(void) {
49 
50  //TODO set gyro bias if used
51 
53 }
54 
55 void ahrs_propagate(void) {
56 #ifdef ADC_CHANNEL_GYRO_P
58 #endif
59 #ifdef ADC_CHANNEL_GYRO_Q
61 #endif
62 #ifdef ADC_CHANNEL_GYRO_R
64 #endif
65 }
66 
67 void ahrs_update_accel(void) {
68 }
69 
70 void ahrs_update_mag(void) {
71 }
72 
73 void ahrs_update_gps(void) {
74 
75  float hspeed_mod_f = gps.gspeed / 100.;
76  float course_f = gps.course / 1e7;
77 
78  // Heading estimator from wind-information, usually computed with -DWIND_INFO
79  // wind_north and wind_east initialized to 0, so still correct if not updated
80  float w_vn = cosf(course_f) * hspeed_mod_f - wind_north;
81  float w_ve = sinf(course_f) * hspeed_mod_f - wind_east;
82  ahrs_float.ltp_to_body_euler.psi = atan2f(w_ve, w_vn);
84  ahrs_float.ltp_to_body_euler.psi += 2 * M_PI;
85 
87 }
88 
91 
93 
94  if (ahrs_float.ltp_to_body_euler.theta < -M_PI_2)
96  else if (ahrs_float.ltp_to_body_euler.theta > M_PI_2)
98 
101  else
103 
106  else
108 
110 }
111 
112 
113 // TODO use ahrs result directly
115 {
116  // export results to estimator
117 
121 
125 
126 }
#define FLOAT_RATES_ZERO(_r)
void ahrs_propagate(void)
Propagation.
Definition: ahrs_infrared.c:55
float roll_neutral
Definition: infrared.h:126
struct FloatRMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:68
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs.h:59
struct Infrared infrared
Definition: infrared.c:36
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
float estimator_q
Definition: estimator.c:55
float estimator_r
Definition: estimator.c:56
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
struct FloatRMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:61
int32_t p
in rad/s^2 with INT32_RATE_FRAC
#define FLOAT_RMAT_ZERO(_rm)
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
float psi
in radians
#define RATE_FLOAT_OF_BFP(_ai)
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
Definition: ahrs_infrared.c:70
float pitch_neutral
Definition: infrared.h:127
float theta
in radians
float correction_left
Definition: infrared.h:143
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
Definition: ahrs_infrared.c:67
float estimator_p
Definition: estimator.c:54
float p
in rad/s^2
void ahrs_update_gps(void)
Definition: ahrs_infrared.c:73
float correction_down
Definition: infrared.h:146
void ahrs_update_infrared(void)
Definition: ahrs_infrared.c:89
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
float phi
in radians
float wind_east
Definition: estimator.c:68
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
struct FloatQuat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:66
Device independent GPS code (interface)
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:54
int16_t roll
Definition: infrared.h:131
Inertial Measurement Unit interface.
struct FloatRates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:62
float r
in rad/s^2
#define AHRS_UNINIT
Definition: ahrs.h:33
void ahrs_update_fw_estimator(void)
#define FLOAT_EULERS_ZERO(_e)
float correction_right
Definition: infrared.h:144
void ahrs_init(void)
AHRS initialization.
Definition: ahrs_infrared.c:32
struct FloatEulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:67
void ahrs_align(void)
Aligns the AHRS.
Definition: ahrs_infrared.c:48
float correction_up
Definition: infrared.h:145
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
State estimation, fusioning sensors.
int32_t q
in rad/s^2 with INT32_RATE_FRAC
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
float wind_north
Definition: estimator.c:68
float q
in rad/s^2
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
struct FloatRates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:69
int32_t r
in rad/s^2 with INT32_RATE_FRAC
int16_t pitch
Definition: infrared.h:132
#define FLOAT_QUAT_ZERO(_q)
int16_t top
Definition: infrared.h:133
struct GpsState gps
global GPS state
Definition: gps.c:31
#define AHRS_RUNNING
Definition: ahrs.h:34
struct FloatEulers ltp_to_imu_euler
Rotation from LocalTangentPlane to IMU frame as Euler angles.
Definition: ahrs.h:60