Skip to content

Commit

Permalink
[ins] clean ins_ardrone2
Browse files Browse the repository at this point in the history
  • Loading branch information
@flixr
flixr committed Sep 5, 2013
1 parent 0845b6f commit 4eced1b
Show file tree
Hide file tree
Showing 2 changed files with 50 additions and 60 deletions.
84 changes: 35 additions & 49 deletions sw/airborne/subsystems/ins/ins_ardrone2.c
View file
Expand Up @@ -41,53 +41,39 @@
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#endif


/* 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;
struct InsArdrone2 ins_impl;

void ins_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 */
llh_nav.lat = INT32_RAD_OF_DEG(NAV_LAT0);
llh_nav.lon = INT32_RAD_OF_DEG(NAV_LON0);
llh_nav.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);

//Convert ltp
ltp_def_from_lla_i(&ins_ltp_def, &llh_nav);
ins_ltp_def.hmsl = NAV_ALT0;
ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
ins_impl.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_impl.ltp_def);

//Set the ltp
stateSetLocalOrigin_i(&ins_ltp_def);

ins_ltp_initialized = TRUE;
ins_impl.ltp_initialized = TRUE;
#else
ins_ltp_initialized = FALSE;
ins_impl.ltp_initialized = FALSE;
#endif

ins.vf_realign = FALSE;
ins.hf_realign = FALSE;

INT32_VECT3_ZERO(ins_ltp_pos);

// TODO correct init
ins.status = INS_RUNNING;
INT32_VECT3_ZERO(ins_impl.ltp_pos);
INT32_VECT3_ZERO(ins_impl.ltp_speed);
INT32_VECT3_ZERO(ins_impl.ltp_accel);
}

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))) {
Expand All @@ -100,21 +86,21 @@ void ins_realign_v(float z __attribute__ ((unused))) {

void ins_propagate() {
/* untilt accels and speeds */
FLOAT_RMAT_VECT3_TRANSP_MUL(ins_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_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel);
FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed);

//Add g to the accelerations
ins_ltp_accel.z += 9.81;
ins_impl.ltp_accel.z += 9.81;

//Save the accelerations and speeds
stateSetAccelNed_f(&ins_ltp_accel);
stateSetSpeedNed_f(&ins_ltp_speed);
stateSetAccelNed_f(&ins_impl.ltp_accel);
stateSetSpeedNed_f(&ins_impl.ltp_speed);

//Don't set the height if we use the one from the gps
#if !USE_GPS_HEIGHT
//Set the height and save the position
ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
stateSetPositionNed_i(&ins_ltp_pos);
ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
stateSetPositionNed_i(&ins_impl.ltp_pos);
#endif
}

Expand All @@ -128,27 +114,27 @@ void ins_update_gps(void) {
//Check for GPS fix
if (gps.fix == GPS_FIX_3D) {
//Set the initial coordinates
if(!ins_ltp_initialized) {
ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
ins_ltp_def.lla.alt = gps.lla_pos.alt;
ins_ltp_def.hmsl = gps.hmsl;
ins_ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_ltp_def);
if(!ins_impl.ltp_initialized) {
ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_impl.ltp_def.hmsl = gps.hmsl;
ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}

//Set the x and y and maybe z position in ltp and save
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
#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
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

//Set the local origin
stateSetPositionNed_i(&ins_ltp_pos);
stateSetPositionNed_i(&ins_impl.ltp_pos);
}
#endif /* USE_GPS */
}
Expand Down
26 changes: 15 additions & 11 deletions sw/airborne/subsystems/ins/ins_ardrone2.h
View file
Expand Up @@ -24,22 +24,26 @@
* INS implementation for ardrone2-sdk.
*/

#ifndef INS_INT_H
#define INS_INT_H
#ifndef INS_ARDRONE2_SDK_H
#define INS_ARDRONE2_SDK_H

#include "subsystems/ins.h"
#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_algebra_float.h"

//TODO: implement in state
extern int32_t ins_qfe;
extern int32_t ins_baro_alt;
struct InsArdrone2 {
struct LtpDef_i ltp_def;
bool_t ltp_initialized;

extern struct NedCoor_i ins_ltp_pos;
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;
float qfe; ///< not used, only dummy for INS_REF message

#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 */

0 comments on commit 4eced1b

Please sign in to comment.