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_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 
33 
35 #include "subsystems/imu.h"
36 #include "subsystems/gps.h"
37 
38 #include "state.h"
39 
40 float heading;
41 
42 void ahrs_init(void) {
44 
45  heading = 0.;
46 }
47 
48 void ahrs_align(void) {
49 
50  //TODO set gyro bias if used
51 
53 }
54 
55 void ahrs_propagate(void) {
56  struct FloatRates body_rate = { 0., 0., 0. };
57 #ifdef ADC_CHANNEL_GYRO_P
58  body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
59 #endif
60 #ifdef ADC_CHANNEL_GYRO_Q
61  body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
62 #endif
63 #ifdef ADC_CHANNEL_GYRO_R
64  body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
65 #endif
66  stateSetBodyRates_f(&body_rate);
67 }
68 
69 void ahrs_update_accel(void) {
70 }
71 
72 void ahrs_update_mag(void) {
73 }
74 
75 void ahrs_update_gps(void) {
76 
77  float hspeed_mod_f = gps.gspeed / 100.;
78  float course_f = gps.course / 1e7;
79 
80  // Heading estimator from wind-information, usually computed with -DWIND_INFO
81  // wind_north and wind_east initialized to 0, so still correct if not updated
82  float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->x;
83  float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y;
84  heading = atan2f(w_ve, w_vn);
85  if (heading < 0.)
86  heading += 2 * M_PI;
87 
88 }
89 
91  float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
92  float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
93 
94  if (theta < -M_PI_2) theta += M_PI;
95  else if (theta > M_PI_2) theta -= M_PI;
96 
97  if (phi >= 0) phi *= infrared.correction_right;
98  else phi *= infrared.correction_left;
99 
100  if (theta >= 0) theta *= infrared.correction_up;
101  else theta *= infrared.correction_down;
102 
103  struct FloatEulers att = { phi, theta, heading };
105 
106 }
107 
void ahrs_propagate(void)
Propagation.
Definition: ahrs_infrared.c:55
float roll_neutral
Definition: infrared.h:124
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
struct Infrared infrared
Definition: infrared.c:34
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
int32_t p
in rad/s^2 with INT32_RATE_FRAC
angular rates
#define RATE_FLOAT_OF_BFP(_ai)
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
Definition: ahrs_infrared.c:72
float pitch_neutral
Definition: infrared.h:125
Fixedwing attitude estimation using infrared sensors.
float theta
in radians
euler angles
float correction_left
Definition: infrared.h:141
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
Definition: ahrs_infrared.c:69
float p
in rad/s^2
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
Definition: ahrs_infrared.c:75
float correction_down
Definition: infrared.h:144
float heading
Definition: ahrs_infrared.c:40
void ahrs_update_infrared(void)
Definition: ahrs_infrared.c:90
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
float phi
in radians
Device independent GPS code (interface)
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
int16_t roll
Definition: infrared.h:129
Inertial Measurement Unit interface.
float r
in rad/s^2
#define AHRS_UNINIT
Definition: ahrs.h:35
float correction_right
Definition: infrared.h:142
void ahrs_init(void)
AHRS initialization.
Definition: ahrs_infrared.c:42
API to get/set the generic vehicle states.
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1192
void ahrs_align(void)
Aligns the AHRS.
Definition: ahrs_infrared.c:48
float correction_up
Definition: infrared.h:143
int32_t q
in rad/s^2 with INT32_RATE_FRAC
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
float q
in rad/s^2
int32_t r
in rad/s^2 with INT32_RATE_FRAC
int16_t pitch
Definition: infrared.h:130
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
int16_t top
Definition: infrared.h:131
struct GpsState gps
global GPS state
Definition: gps.c:33
#define AHRS_RUNNING
Definition: ahrs.h:36