Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 #include "subsystems/abi.h"
40 
41 #ifndef INFRARED_FILTER_ID
42 #define INFRARED_FILTER_ID 2
43 #endif
44 
45 static float heading;
46 
50 #ifndef AHRS_INFRARED_GYRO_ID
51 #define AHRS_INFRARED_GYRO_ID ABI_BROADCAST
52 #endif
54 
55 #ifndef AHRS_INFRARED_GPS_ID
56 #define AHRS_INFRARED_GPS_ID GPS_MULTI_ID
57 #endif
59 void ahrs_infrared_update_gps(struct GpsState *gps_s);
60 
61 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
62  uint32_t stamp __attribute__((unused)),
63  struct Int32Rates *gyro)
64 {
65  stateSetBodyRates_i(gyro);
66 }
67 
68 static void gps_cb(uint8_t sender_id __attribute__((unused)),
69  uint32_t stamp __attribute__((unused)),
70  struct GpsState *gps_s)
71 {
73 }
74 
75 
76 #if PERIODIC_TELEMETRY
78 
79 static void send_infrared(struct transport_tx *trans, struct link_device *dev)
80 {
81  pprz_msg_send_IR_SENSORS(trans, dev, AC_ID,
83 }
84 
85 static void send_status(struct transport_tx *trans, struct link_device *dev)
86 {
87  uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top);
88  uint8_t mde = 3;
90  if (contrast < 50) { mde = 7; }
91  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &contrast);
92 }
93 #endif
94 
95 
97 {
98  heading = 0.;
99 
100  AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb);
101  AbiBindMsgGPS(AHRS_INFRARED_GPS_ID, &gps_ev, &gps_cb);
102 
103 #if PERIODIC_TELEMETRY
105  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_status);
106 #endif
107 }
108 
109 
111 {
112  float hspeed_mod_f = gps_s->gspeed / 100.;
113  float course_f = gps_s->course / 1e7;
114 
115  // Heading estimator from wind-information, usually computed with -DWIND_INFO
116  // wind_north and wind_east initialized to 0, so still correct if not updated
117  float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->x;
118  float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y;
119  heading = atan2f(w_ve, w_vn);
120  if (heading < 0.) {
121  heading += 2 * M_PI;
122  }
123 
124 }
125 
127 {
128  float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
129  float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
130 
131  if (theta < -M_PI_2) { theta += M_PI; }
132  else if (theta > M_PI_2) { theta -= M_PI; }
133 
134  if (phi >= 0) { phi *= infrared.correction_right; }
135  else { phi *= infrared.correction_left; }
136 
137  if (theta >= 0) { theta *= infrared.correction_up; }
138  else { theta *= infrared.correction_down; }
139 
140  struct FloatEulers att = { phi, theta, heading };
142 }
Infrared::pitch
int16_t pitch
Definition: infrared.h:130
uint16_t
unsigned short uint16_t
Definition: types.h:16
Infrared::top
int16_t top
Definition: infrared.h:131
Infrared_raw::ir1
int16_t ir1
Definition: infrared.h:112
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
Infrared::value
struct Infrared_raw value
Definition: infrared.h:121
abi.h
GpsState
data structure for GPS information
Definition: gps.h:87
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
uint32_t
unsigned long uint32_t
Definition: types.h:18
gps_ev
static abi_event gps_ev
Definition: ahrs_infrared.c:58
gyro_ev
static abi_event gyro_ev
Definition: ahrs_infrared.c:53
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
telemetry.h
Infrared::correction_left
float correction_left
Definition: infrared.h:141
imu.h
stateSetBodyRates_i
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1173
INFRARED_FILTER_ID
#define INFRARED_FILTER_ID
Definition: ahrs_infrared.c:42
Infrared_raw::ir2
int16_t ir2
Definition: infrared.h:113
gps.h
Device independent GPS code (interface)
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
infrared
struct Infrared infrared
Definition: infrared.c:30
Infrared::correction_up
float correction_up
Definition: infrared.h:143
ahrs_infrared.h
uint8_t
unsigned char uint8_t
Definition: types.h:14
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Infrared::roll
int16_t roll
Definition: infrared.h:129
send_infrared
static void send_infrared(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_infrared.c:79
AHRS_INFRARED_GYRO_ID
#define AHRS_INFRARED_GYRO_ID
ABI binding for gyro data.
Definition: ahrs_infrared.c:51
heading
static float heading
Definition: ahrs_infrared.c:45
Infrared::pitch_neutral
float pitch_neutral
Definition: infrared.h:125
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
stateGetHorizontalWindspeed_f
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
stateSetNedToBodyEulers_f
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ahrs_infrared.c:68
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
send_status
static void send_status(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_infrared.c:85
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
Infrared::correction_down
float correction_down
Definition: infrared.h:144
state.h
Infrared::correction_right
float correction_right
Definition: infrared.h:142
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition: ahrs_infrared.c:61
ahrs_infrared_update_gps
void ahrs_infrared_update_gps(struct GpsState *gps_s)
Definition: ahrs_infrared.c:110
ahrs_infrared_periodic
void ahrs_infrared_periodic(void)
Definition: ahrs_infrared.c:126
ahrs_infrared_init
void ahrs_infrared_init(void)
Definition: ahrs_infrared.c:96
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
infrared.h
Infrared::roll_neutral
float roll_neutral
Definition: infrared.h:124
AHRS_INFRARED_GPS_ID
#define AHRS_INFRARED_GPS_ID
Definition: ahrs_infrared.c:56