Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-03 21:33:39 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[ins] clean ins_ardrone2

Browse Source
This commit is contained in:
Felix Ruess
2013-09-05 19:17:10 +02:00
parent 0845b6f533
commit 4eced1b096
2 changed files with 50 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ins/ins_ardrone2.c
+35 -49
View File
@@ -41,53 +41,39 @@
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#endif #endif
struct InsArdrone2 ins_impl;
/* TODO: implement in state */
int32_t ins_qfe;
int32_t ins_baro_alt;
//Keep track of gps pos and the init pos
struct NedCoor_i ins_ltp_pos;
struct LtpDef_i ins_ltp_def;
// Keep track of INS LTP accel and speed
struct NedCoor_f ins_ltp_accel;
struct NedCoor_f ins_ltp_speed;
bool_t ins_ltp_initialized;
void ins_init() { void ins_init() {
#if USE_INS_NAV_INIT #if USE_INS_NAV_INIT
struct LlaCoor_i llh_nav; struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
/** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ struct EcefCoor_i ecef_nav0;
llh_nav.lat = INT32_RAD_OF_DEG(NAV_LAT0); ecef_of_lla_i(&ecef_nav0, &llh_nav0);
llh_nav.lon = INT32_RAD_OF_DEG(NAV_LON0);
llh_nav.alt = NAV_ALT0 + NAV_MSL0;
//Convert ltp ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
ltp_def_from_lla_i(&ins_ltp_def, &llh_nav); ins_impl.ltp_def.hmsl = NAV_ALT0;
ins_ltp_def.hmsl = NAV_ALT0; stateSetLocalOrigin_i(&ins_impl.ltp_def);
//Set the ltp ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_ltp_def);
ins_ltp_initialized = TRUE;
#else #else
ins_ltp_initialized = FALSE; ins_impl.ltp_initialized = FALSE;
#endif #endif
ins.vf_realign = FALSE; ins.vf_realign = FALSE;
ins.hf_realign = FALSE; ins.hf_realign = FALSE;
INT32_VECT3_ZERO(ins_ltp_pos); INT32_VECT3_ZERO(ins_impl.ltp_pos);
INT32_VECT3_ZERO(ins_impl.ltp_speed);
// TODO correct init INT32_VECT3_ZERO(ins_impl.ltp_accel);
ins.status = INS_RUNNING;
} }
void ins_periodic( void ) { void ins_periodic( void ) {
if (ins_impl.ltp_initialized)
ins.status = INS_RUNNING;
} }
void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
@@ -100,21 +86,21 @@ void ins_realign_v(float z __attribute__ ((unused))) {
void ins_propagate() { void ins_propagate() {
/* untilt accels and speeds */ /* untilt accels and speeds */
FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel); FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel);
FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed); FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed);
//Add g to the accelerations //Add g to the accelerations
ins_ltp_accel.z += 9.81; ins_impl.ltp_accel.z += 9.81;
//Save the accelerations and speeds //Save the accelerations and speeds
stateSetAccelNed_f(&ins_ltp_accel); stateSetAccelNed_f(&ins_impl.ltp_accel);
stateSetSpeedNed_f(&ins_ltp_speed); stateSetSpeedNed_f(&ins_impl.ltp_speed);
//Don't set the height if we use the one from the gps //Don't set the height if we use the one from the gps
#if !USE_GPS_HEIGHT #if !USE_GPS_HEIGHT
//Set the height and save the position //Set the height and save the position
ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
stateSetPositionNed_i(&ins_ltp_pos); stateSetPositionNed_i(&ins_impl.ltp_pos);
#endif #endif
} }
@@ -128,27 +114,27 @@ void ins_update_gps(void) {
//Check for GPS fix //Check for GPS fix
if (gps.fix == GPS_FIX_3D) { if (gps.fix == GPS_FIX_3D) {
//Set the initial coordinates //Set the initial coordinates
if(!ins_ltp_initialized) { if(!ins_impl.ltp_initialized) {
ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
ins_ltp_def.lla.alt = gps.lla_pos.alt; ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_ltp_def.hmsl = gps.hmsl; ins_impl.ltp_def.hmsl = gps.hmsl;
ins_ltp_initialized = TRUE; ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_ltp_def); stateSetLocalOrigin_i(&ins_impl.ltp_def);
} }
//Set the x and y and maybe z position in ltp and save //Set the x and y and maybe z position in ltp and save
struct NedCoor_i ins_gps_pos_cm_ned; struct NedCoor_i ins_gps_pos_cm_ned;
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
//When we don't want to use the height of the navdata we can use the gps height //When we don't want to use the height of the navdata we can use the gps height
#if USE_GPS_HEIGHT #if USE_GPS_HEIGHT
INT32_VECT3_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT3_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
#else #else
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT2_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
#endif #endif
//Set the local origin //Set the local origin
stateSetPositionNed_i(&ins_ltp_pos); stateSetPositionNed_i(&ins_impl.ltp_pos);
} }
#endif /* USE_GPS */ #endif /* USE_GPS */
} }
sw/airborne/subsystems/ins/ins_ardrone2.h
+15 -11
View File
@@ -24,22 +24,26 @@
* INS implementation for ardrone2-sdk. * INS implementation for ardrone2-sdk.
*/ */
#ifndef INS_INT_H #ifndef INS_ARDRONE2_SDK_H
#define INS_INT_H #define INS_ARDRONE2_SDK_H
#include "subsystems/ins.h" #include "subsystems/ins.h"
#include "std.h" #include "std.h"
#include "math/pprz_geodetic_int.h" #include "math/pprz_geodetic_int.h"
#include "math/pprz_algebra_float.h" #include "math/pprz_algebra_float.h"
//TODO: implement in state struct InsArdrone2 {
extern int32_t ins_qfe; struct LtpDef_i ltp_def;
extern int32_t ins_baro_alt; bool_t ltp_initialized;
extern struct NedCoor_i ins_ltp_pos; float qfe; ///< not used, only dummy for INS_REF message
extern struct LtpDef_i ins_ltp_def;
extern struct NedCoor_f ins_ltp_speed;
extern struct NedCoor_f ins_ltp_accel;
extern bool_t ins_ltp_initialized;
#endif /* INS_INT_H */ /* output LTP NED */
struct NedCoor_i ltp_pos;
struct NedCoor_i ltp_speed;
struct NedCoor_i ltp_accel;
};
extern struct InsArdrone2 ins_impl;
#endif /* INS_ARDRONE2_SDK_H */