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
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 
45 
46 void ins_init() {
47 #if USE_INS_NAV_INIT
48  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
49  llh_nav0.lat = NAV_LAT0;
50  llh_nav0.lon = NAV_LON0;
51  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
52  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
53 
54  struct EcefCoor_i ecef_nav0;
55  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
56 
57  ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
58  ins_impl.ltp_def.hmsl = NAV_ALT0;
60 
62 #else
64 #endif
65 
69 }
70 
71 void ins_periodic( void ) {
74 }
75 
76 void ins_reset_local_origin( void ) {
78 }
79 
80 void ins_reset_altitude_ref( void ) {
81 #if USE_GPS
82  struct LlaCoor_i lla = {
86  };
90 #endif
91 }
92 
93 void ins_propagate() {
94  /* untilt accels and speeds */
97 
98  //Add g to the accelerations
99  ins_impl.ltp_accel.z += 9.81;
100 
101  //Save the accelerations and speeds
104 
105  //Don't set the height if we use the one from the gps
106 #if !USE_GPS_HEIGHT
107  //Set the height and save the position
110 #endif
111 }
112 
113 
114 void ins_update_gps(void) {
115 #if USE_GPS
116  //Check for GPS fix
117  if (gps.fix == GPS_FIX_3D) {
118  //Set the initial coordinates
125  }
126 
127  //Set the x and y and maybe z position in ltp and save
128  struct NedCoor_i ins_gps_pos_cm_ned;
129  ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
130 
131  //When we don't want to use the height of the navdata we can use the gps height
132 #if USE_GPS_HEIGHT
134 #else
136 #endif
137 
138  //Set the local origin
140  }
141 #endif /* USE_GPS */
142 }
struct Ins ins
global INS state
Definition: ins.c:36
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: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
int32_t lat
in degrees*1e7
Attitude and Heading Reference System interface.
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
struct LtpDef_i ltp_def
Definition: ins_ardrone2.h:36
vector in EarthCenteredEarthFixed coordinates
#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
INS implementation for ardrone2-sdk.
enum InsStatus status
status of the INS
Definition: ins.h:47
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
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_ardrone2.c:76
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
void ins_propagate()
Propagation.
Definition: ins_ardrone2.c:93
struct InsArdrone2 ins_impl
global INS state
Definition: ins_ardrone2.c:44
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
#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:71
vector in North East Down coordinates
int32_t z
Down.
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:754
struct NedCoor_f speed
Definition: ahrs_ardrone2.h:42
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_ardrone2.c:80
#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 ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct NedCoor_i ltp_pos
Definition: ins_ardrone2.h:42
Paparazzi fixed point math for geodetic calculations.
#define INT32_POS_OF_CM_DEN
#define TRUE
Definition: imu_chimu.h:144
void ins_init()
INS initialization.
Definition: ins_ardrone2.c:46
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
struct NedCoor_f ltp_accel
Definition: ins_ardrone2.h:44
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:114
int32_t lon
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
#define INT32_POS_OF_CM_NUM
struct AhrsARDrone ahrs_impl
Definition: ahrs_ardrone2.c:45
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:908
struct NedCoor_f ltp_speed
Definition: ins_ardrone2.h:43
struct GpsState gps
global GPS state
Definition: gps.c:41
struct State state
Definition: state.c:36
bool_t ltp_initialized
Definition: ins_ardrone2.h:37