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
ins_ardrone2.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Freek van Tienen
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 
28 #include "subsystems/ahrs.h"
29 #include "subsystems/gps.h"
30 #include "generated/airframe.h"
31 #include "generated/flight_plan.h"
32 #include "math/pprz_geodetic_int.h"
33 
34 #ifdef SITL
35 #include "nps_fdm.h"
36 #include <stdio.h>
37 #endif
38 
39 #ifndef USE_INS_NAV_INIT
40 #define USE_INS_NAV_INIT TRUE
41 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
42 #endif
43 
44 
45 /* TODO: implement in state */
48 
49 //Keep track of gps pos and the init pos
52 
53 // Keep track of INS LTP accel and speed
56 
58 
59 void ins_init() {
60 #if USE_INS_NAV_INIT
61  struct LlaCoor_i llh_nav;
62 
64  llh_nav.lat = INT32_RAD_OF_DEG(NAV_LAT0);
65  llh_nav.lon = INT32_RAD_OF_DEG(NAV_LON0);
66  llh_nav.alt = NAV_ALT0 + NAV_MSL0;
67 
68  //Convert ltp
69  ltp_def_from_lla_i(&ins_ltp_def, &llh_nav);
70  ins_ltp_def.hmsl = NAV_ALT0;
71 
72  //Set the ltp
74 
75  ins_ltp_initialised = TRUE;
76 #else
77  ins_ltp_initialised = FALSE;
78 #endif
79 
82 
84 
85  // TODO correct init
87 }
88 
89 void ins_periodic( void ) {
90 
91 }
92 
93 void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
94 
95 }
96 
97 void ins_realign_v(float z __attribute__ ((unused))) {
98 
99 }
100 
102  /* untilt accels and speeds */
105 
106  //Add g to the accelerations
107  ins_ltp_accel.z += 9.81;
108 
109  //Save the accelerations and speeds
112 
113  //Don't set the height if we use the one from the gps
114 #if !USE_GPS_HEIGHT
115  //Set the height and save the position
118 #endif
119 }
120 
122 
123 }
124 
125 
126 void ins_update_gps(void) {
127 #if USE_GPS
128  //Check for GPS fix
129  if (gps.fix == GPS_FIX_3D) {
130  //Set the initial coordinates
131  if(!ins_ltp_initialised) {
135  ins_ltp_initialised = TRUE;
137  }
138 
139  //Set the x and y and maybe z position in ltp and save
140  struct NedCoor_i ins_gps_pos_cm_ned;
141  ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
142 
143  //When we don't want to use the height of the navdata we can use the gps height
144 #if USE_GPS_HEIGHT
146 #else
148 #endif
149 
150  //Set the local origin
152  }
153 #endif /* USE_GPS */
154 }
155 
157 
158 }
struct Ins ins
global INS state
Definition: ins.c:30
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:508
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
int32_t lat
in radians*1e7
Attitude and Heading Reference System interface.
int32_t ins_qfe
Definition: ins_ardrone2.c:46
int32_t ins_baro_alt
Definition: ins_ardrone2.c:47
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
struct NedCoor_f ins_ltp_speed
Definition: ins_ardrone2.c:55
#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
INS implementation for ardrone2-sdk.
#define INS_RUNNING
Definition: ins.h:36
struct LlaCoor_i lla
Reference point in lla.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
vector in Latitude, Longitude and Altitude
struct NedCoor_f accel
Definition: ahrs_ardrone2.h:43
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
int32_t altitude
Definition: ahrs_ardrone2.h:44
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
struct NedCoor_f ins_ltp_accel
Definition: ins_ardrone2.c:54
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
bool_t hf_realign
realign horizontally if true
Definition: ins.h:46
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
bool_t ins_ltp_initialised
Definition: ins_ardrone2.c:57
void ins_propagate()
Propagation.
Definition: ins_ardrone2.c:101
#define FALSE
Definition: imu_chimu.h:141
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1022
void ins_periodic(void)
INS periodic call.
Definition: ins_ardrone2.c:89
vector in North East Down coordinates
int32_t z
Down.
uint8_t status
status of the INS
Definition: ins.h:45
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:754
void ins_update_sonar()
Update INS state with sonar measurements.
Definition: ins_ardrone2.c:156
definition of the local (flat earth) coordinate system
#define INT32_RAD_OF_DEG(_deg)
struct NedCoor_f speed
Definition: ahrs_ardrone2.h:42
void ins_realign_v(float z)
INS vertical realign.
Definition: ins_ardrone2.c:97
vector in North East Down coordinates Units: meters
#define INT32_VECT3_ZERO(_v)
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:440
Device independent GPS code (interface)
float z
in meters
void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed)
INS horizontal realign.
Definition: ins_ardrone2.c:93
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
int32_t alt
in millimeters above WGS84 reference ellipsoid
Paparazzi fixed point math for geodetic calculations.
#define INT32_POS_OF_CM_DEN
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
void ins_init()
INS initialization.
Definition: ins_ardrone2.c:59
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
void ins_update_gps(void)
Update INS state with GPS measurements.
Definition: ins_ardrone2.c:126
int32_t lon
in radians*1e7
int32_t hmsl
Height above mean sea level in mm.
struct NedCoor_i ins_ltp_pos
Definition: ins_ardrone2.c:50
struct LtpDef_i ins_ltp_def
Ins implementation state (fixed point)
Definition: ins_ardrone2.c:51
#define INT32_POS_OF_CM_NUM
bool_t vf_realign
realign vertically if true
Definition: ins.h:47
void ins_update_baro()
Update INS state with barometer measurements.
Definition: ins_ardrone2.c:121
struct AhrsARDrone ahrs_impl
Definition: ahrs_ardrone2.c:40
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:908
struct GpsState gps
global GPS state
Definition: gps.c:33