Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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 #if PERIODIC_TELEMETRY
44 
45 static void send_infrared(void) {
46  DOWNLINK_SEND_IR_SENSORS(DefaultChannel, DefaultDevice,
48 }
49 
50 static void send_status(void) {
51  uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top);
52  uint8_t mde = 3;
53  if (contrast < 50) mde = 7;
54  DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &contrast);
55 }
56 #endif
57 
58 void ahrs_init(void) {
60 
61  heading = 0.;
62 
63 #if PERIODIC_TELEMETRY
64  register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
65  register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status);
66 #endif
67 }
68 
69 void ahrs_align(void) {
70 
71  //TODO set gyro bias if used
72 
74 }
75 
76 void ahrs_propagate(void) {
77  struct FloatRates body_rate = { 0., 0., 0. };
78 #ifdef ADC_CHANNEL_GYRO_P
79  body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
80 #endif
81 #ifdef ADC_CHANNEL_GYRO_Q
82  body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
83 #endif
84 #ifdef ADC_CHANNEL_GYRO_R
85  body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
86 #endif
87  stateSetBodyRates_f(&body_rate);
88 }
89 
90 
91 void ahrs_update_gps(void) {
92 
93  float hspeed_mod_f = gps.gspeed / 100.;
94  float course_f = gps.course / 1e7;
95 
96  // Heading estimator from wind-information, usually computed with -DWIND_INFO
97  // wind_north and wind_east initialized to 0, so still correct if not updated
98  float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->x;
99  float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y;
100  heading = atan2f(w_ve, w_vn);
101  if (heading < 0.)
102  heading += 2 * M_PI;
103 
104 }
105 
107  float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
108  float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
109 
110  if (theta < -M_PI_2) theta += M_PI;
111  else if (theta > M_PI_2) theta -= M_PI;
112 
113  if (phi >= 0) phi *= infrared.correction_right;
114  else phi *= infrared.correction_left;
115 
116  if (theta >= 0) theta *= infrared.correction_up;
117  else theta *= infrared.correction_down;
118 
119  struct FloatEulers att = { phi, theta, heading };
121 
122 }
123 
void ahrs_propagate(void)
Propagation.
Definition: ahrs_infrared.c:76
unsigned short uint16_t
Definition: types.h:16
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 with INT32_RATE_FRAC
Periodic telemetry system header (includes downlink utility and generated code).
angular rates
#define RATE_FLOAT_OF_BFP(_ai)
int16_t ir2
Definition: infrared.h:113
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
float pitch_neutral
Definition: infrared.h:125
Fixedwing attitude estimation using infrared sensors.
float theta
in radians
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
euler angles
float correction_left
Definition: infrared.h:141
float p
in rad/s
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
Definition: ahrs_infrared.c:91
float correction_down
Definition: infrared.h:144
float heading
Definition: ahrs_infrared.c:40
void ahrs_update_infrared(void)
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:47
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
#define AHRS_UNINIT
Definition: ahrs.h:35
float correction_right
Definition: infrared.h:142
void ahrs_init(void)
AHRS initialization.
Definition: ahrs_infrared.c:58
unsigned char uint8_t
Definition: types.h:14
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:69
struct Infrared_raw value
Definition: infrared.h:121
float correction_up
Definition: infrared.h:143
int16_t ir1
Definition: infrared.h:112
int32_t q
in rad/s with INT32_RATE_FRAC
static void send_status(void)
Definition: autopilot.c:143
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:41
float q
in rad/s
int32_t r
in rad/s 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:41
#define AHRS_RUNNING
Definition: ahrs.h:36