Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ahrs_sim.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-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 
22 
23 #include "subsystems/ahrs.h"
26 #include "generated/airframe.h"
27 
28 #include <inttypes.h>
29 #include <caml/mlvalues.h>
30 
31 extern float sim_phi;
32 extern float sim_theta;
33 extern float sim_psi;
34 extern float sim_p;
35 extern float sim_q;
36 extern float sim_r;
37 extern bool_t ahrs_sim_available;
38 
39 
41 
46 
50 
51  /* set quaternion and rotation matrix representations as well */
54 
56 }
57 
58 
59 void ahrs_init(void) {
60  //ahrs_float.status = AHRS_UNINIT;
61  // set to running for now and only use ahrs.status, not ahrs_float.status
63 
65 
66  /* set ltp_to_body to zero */
71 
72  /* set ltp_to_imu to same as ltp_to_body, currently no difference simulated */
77 }
78 
79 void ahrs_align(void)
80 {
81  /* Currently not really simulated
82  * body and imu have the same frame and always set to true value from sim
83  */
84 
86 
87  /* Compute initial body orientation */
89 
91 }
92 
93 
94 void ahrs_propagate(void) {
95 }
96 
97 void ahrs_update_accel(void) {
98 }
99 
100 void ahrs_update_mag(void) {
101 }
102 
103 void ahrs_update_gps(void) {
104 
105 }
106 
107 
108 /*
109  * Compute body orientation and rates from imu orientation and rates
110  */
112 
113  /* set ltp_to_body to same as ltp_to_imu, currently no difference simulated */
114 
119 }
120 
121 
122 #ifdef AHRS_UPDATE_FW_ESTIMATOR
123 // TODO use ahrs result directly
124 #include "estimator.h"
125 // remotely settable
126 #ifndef INS_ROLL_NEUTRAL_DEFAULT
127 #define INS_ROLL_NEUTRAL_DEFAULT 0
128 #endif
129 #ifndef INS_PITCH_NEUTRAL_DEFAULT
130 #define INS_PITCH_NEUTRAL_DEFAULT 0
131 #endif
132 float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
133 float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
134 void ahrs_update_fw_estimator(void)
135 {
136  // really subtract ins neutrals here?
140 
144 }
145 #endif //AHRS_UPDATE_FW_ESTIMATOR
#define FLOAT_RATES_ZERO(_r)
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
bool_t ahrs_sim_available
Definition: jsbsim_ahrs.c:17
void ahrs_update_gps(void)
Definition: ahrs_sim.c:103
Attitude and Heading Reference System interface.
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
struct FloatRMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:61
#define FLOAT_RMAT_ZERO(_rm)
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
Definition: ahrs_sim.c:97
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
float psi
in radians
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:303
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
void ahrs_propagate(void)
Propagation.
Definition: ahrs_sim.c:94
float theta
in radians
#define FLOAT_QUAT_OF_EULERS(_q, _e)
float estimator_p
Definition: estimator.c:54
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
Definition: ahrs_sim.c:100
float sim_phi
in radians
Definition: jsbsim_ahrs.c:11
#define FALSE
Definition: imu_chimu.h:141
float p
in rad/s^2
Paparazzi floating point algebra.
float phi
in radians
#define RMAT_COPY(_o, _i)
Definition: pprz_algebra.h:564
void compute_body_orientation_and_rates(void)
Definition: ahrs_sim.c:111
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
float sim_theta
in radians
Definition: jsbsim_ahrs.c:12
struct FloatQuat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:66
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:54
void update_ahrs_from_sim(void)
Definition: ahrs_sim.c:42
float sim_r
in radians/s
Definition: jsbsim_ahrs.c:16
float sim_q
in radians/s
Definition: jsbsim_ahrs.c:15
void ahrs_align(void)
Aligns the AHRS.
Definition: ahrs_sim.c:79
struct FloatRates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:62
float ins_pitch_neutral
Definition: ins_arduimu.c:15
float r
in rad/s^2
void ahrs_update_fw_estimator(void)
#define FLOAT_EULERS_ZERO(_e)
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:478
struct FloatEulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:67
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
State estimation, fusioning sensors.
float q
in rad/s^2
float ins_roll_neutral
Definition: ins_arduimu.c:14
struct FloatRates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:69
#define FLOAT_QUAT_ZERO(_q)
float sim_p
in radians/s
Definition: jsbsim_ahrs.c:14
float sim_psi
in radians
Definition: jsbsim_ahrs.c:13
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:236
#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
void ahrs_init(void)
AHRS initialization.
Definition: ahrs_sim.c:59