|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
INS for rotorcrafts combining vertical and horizontal filters. More...
#include "modules/ins/ins_int.h"#include "modules/core/abi.h"#include "modules/imu/imu.h"#include "modules/gps/gps.h"#include "generated/airframe.h"#include "modules/ins/vf_float.h"#include "math/pprz_geodetic_int.h"#include "math/pprz_isa.h"#include "math/pprz_stat.h"#include "modules/datalink/telemetry.h"Go to the source code of this file.
Macros | |
| #define | VFF_R_AGL 0.2 |
| #define | INS_VFF_R_GPS 2.0 |
| #define | INS_VFF_VZ_R_GPS 2.0 |
| #define | INS_MAX_PROPAGATION_STEPS 200 |
| maximum number of propagation steps without any updates in between | |
| #define | USE_INS_NAV_INIT TRUE |
| #define | INS_BARO_MAX_INIT_VAR 1.f |
| default barometer to use in INS | |
| #define | INS_INT_BARO_ID AIR_DATA_SENDER_ID |
| #define | INS_INT_IMU_ID ABI_BROADCAST |
| ABI binding for IMU data. | |
| #define | INS_INT_GPS_ID GPS_MULTI_ID |
| #define | INS_INT_VEL_ID ABI_BROADCAST |
| ABI binding for VELOCITY_ESTIMATE. | |
| #define | INS_INT_POS_ID ABI_BROADCAST |
| #define | INS_INT_AGL_ID ABI_BROADCAST |
| ABI binding for AGL. | |
| #define | press_hist_len 10 |
Functions | |
| static void | baro_cb (uint8_t sender_id, uint32_t stamp, float pressure) |
| static void | accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel) |
| static void | gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s) |
| static void | vel_est_cb (uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z) |
| static void | pos_est_cb (uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z) |
| static void | agl_cb (uint8_t sender_id, uint32_t stamp, float distance) |
| agl_cb This callback handles all estimates of the height of the vehicle above the ground under it This is only used with the extended version of the vertical float filter | |
| static void | reset_cb (uint8_t sender_id, uint8_t flag) |
| static void | send_ins (struct transport_tx *trans, struct link_device *dev) |
| static void | send_ins_z (struct transport_tx *trans, struct link_device *dev) |
| static void | send_ins_ref (struct transport_tx *trans, struct link_device *dev) |
| static void | ins_ned_to_state (void) |
| copy position and speed to state interface | |
| static void | ins_update_from_vff (void) |
| update ins state from vertical filter | |
| void | ins_int_init (void) |
| static void | reset_ref (void) |
| static void | reset_vertical_ref (void) |
| static void | reset_vertical_pos (void) |
| void | ins_int_propagate (struct Int32Vect3 *accel, float dt) |
| void | ins_int_update_gps (struct GpsState *gps_s) |
Variables | |
| abi_event | baro_ev |
| static abi_event | accel_ev |
| static abi_event | gps_ev |
| static abi_event | vel_est_ev |
| static abi_event | pos_est_ev |
| static abi_event | agl_ev |
| The agl ABI event. | |
| static abi_event | reset_ev |
| struct InsInt | ins_int |
| global INS state | |
INS for rotorcrafts combining vertical and horizontal filters.
Definition in file ins_int.c.
| #define INS_BARO_MAX_INIT_VAR 1.f |
| #define INS_INT_AGL_ID ABI_BROADCAST |
ABI binding for AGL.
Usually this is comes from sonar or gps.
Definition at line 156 of file ins_int.c.
Referenced by ins_int_init().
| #define INS_INT_BARO_ID AIR_DATA_SENDER_ID |
Definition at line 107 of file ins_int.c.
Referenced by ins_int_init().
| #define INS_INT_GPS_ID GPS_MULTI_ID |
Definition at line 124 of file ins_int.c.
Referenced by ins_int_init().
| #define INS_INT_IMU_ID ABI_BROADCAST |
ABI binding for IMU data.
Used accel ABI messages.
Definition at line 117 of file ins_int.c.
Referenced by ins_int_init().
| #define INS_INT_POS_ID ABI_BROADCAST |
Definition at line 143 of file ins_int.c.
Referenced by ins_int_init().
| #define INS_INT_VEL_ID ABI_BROADCAST |
ABI binding for VELOCITY_ESTIMATE.
Usually this is coming from opticflow.
Definition at line 134 of file ins_int.c.
Referenced by ins_int_init().
| #define INS_MAX_PROPAGATION_STEPS 200 |
maximum number of propagation steps without any updates in between
Definition at line 96 of file ins_int.c.
Referenced by ins_int_init(), and ins_int_propagate().
| #define INS_VFF_R_GPS 2.0 |
Definition at line 86 of file ins_int.c.
Referenced by ins_int_update_gps().
| #define INS_VFF_VZ_R_GPS 2.0 |
Definition at line 90 of file ins_int.c.
Referenced by ins_int_update_gps().
| #define press_hist_len 10 |
Referenced by baro_cb().
|
static |
Definition at line 573 of file ins_int.c.
References ins_int_propagate(), and PRINT_CONFIG_MSG().
Definition at line 359 of file ins_int.c.
References idx, INS_BARO_MAX_INIT_VAR, ins_int, ins_update_from_vff(), pprz_isa_height_of_pressure(), press_hist_len, stateGetPositionEnu_f(), variance_f(), vff_realign(), vff_update(), vff_update_baro(), EnuCoor_f::x, and EnuCoor_f::y.
Definition at line 587 of file ins_int.c.
References ins_int_update_gps().
| void ins_int_init | ( | void | ) |
Definition at line 203 of file ins_int.c.
References ABI_BROADCAST, accel_cb(), accel_ev, agl_cb(), agl_ev, baro_cb(), baro_ev, DefaultPeriodic, gps_cb(), gps_ev, hff_init(), ins_init_origin_i_from_flightplan(), ins_int, INS_INT_AGL_ID, INS_INT_BARO_ID, INS_INT_GPS_ID, INS_INT_IMU_ID, INS_INT_POS_ID, INS_INT_VEL_ID, INS_MAX_PROPAGATION_STEPS, INT32_VECT3_ZERO, pos_est_cb(), pos_est_ev, register_periodic_telemetry(), reset_cb(), reset_ev, send_ins(), send_ins_ref(), send_ins_z(), vel_est_cb(), vel_est_ev, and vff_init_zero().
| void ins_int_propagate | ( | struct Int32Vect3 * | accel, |
| float | dt ) |
Definition at line 315 of file ins_int.c.
References ACCEL_BFP_OF_REAL, ACCEL_FLOAT_OF_BFP, hff_propagate(), ins_int, INS_MAX_PROPAGATION_STEPS, ins_ned_to_state(), ins_update_from_vff(), int32_rmat_transp_vmult(), stateGetNedToBodyRMat_i(), stateSetAccelBody_i(), vff_propagate(), Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.
Referenced by accel_cb().
| void ins_int_update_gps | ( | struct GpsState * | gps_s | ) |
Definition at line 418 of file ins_int.c.
References ecef_int_from_gps(), ecef_vel_int_from_gps(), GpsState::fix, GPS_FIX_3D, hff_realign(), hff_update_gps(), ins_int, ins_ned_to_state(), INS_VFF_R_GPS, INS_VFF_VZ_R_GPS, INT32_POS_OF_CM_DEN, INT32_POS_OF_CM_NUM, int32_quat_vmult(), INT32_SPEED_OF_CM_S_DEN, INT32_SPEED_OF_CM_S_NUM, INT32_VECT2_SCALE_2, ned_of_ecef_point_i(), ned_of_ecef_vect_i(), QUAT_INVERT, reset_ref(), stateGetNedToBodyQuat_i(), VECT2_ASSIGN, VECT2_SDIV, VECT3_SUB, vff_update_vz_conf(), vff_update_z_conf(), NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.
Referenced by gps_cb().
|
static |
copy position and speed to state interface
Definition at line 538 of file ins_int.c.
References ins_int, nps_bypass_ins, sim_overwrite_ins(), stateSetAccelNed_i(), stateSetPositionNed_i(), and stateSetSpeedNed_i().
Referenced by ins_int_propagate(), ins_int_update_gps(), ins_ned_to_state(), pos_est_cb(), and vel_est_cb().
|
static |
update ins state from vertical filter
Definition at line 552 of file ins_int.c.
References ACCEL_BFP_OF_REAL, ins_int, POS_BFP_OF_REAL, SPEED_BFP_OF_REAL, and vff.
Referenced by baro_cb(), and ins_int_propagate().
|
static |
Definition at line 659 of file ins_int.c.
References hff_update_pos(), ins_int, ins_ned_to_state(), POS_BFP_OF_REAL, vff_update_z_conf(), FloatVect2::x, and FloatVect2::y.
Referenced by ins_int_init().
Definition at line 297 of file ins_int.c.
References INS_RESET_REF, INS_RESET_VERTICAL_POS, INS_RESET_VERTICAL_REF, reset_ref(), reset_vertical_pos(), reset_vertical_ref(), and UNUSED.
|
static |
Definition at line 250 of file ins_int.c.
References LlaCoor_i::alt, ecef_int_from_gps(), gps, GpsFixValid, ins_int, lla_int_from_gps(), ltp_def_from_ecef_i(), and stateSetLocalOrigin_i().
Referenced by ins_int_update_gps(), and reset_cb().
|
static |
Definition at line 292 of file ins_int.c.
References ins_int.
Referenced by reset_cb().
|
static |
Definition at line 274 of file ins_int.c.
References LlaCoor_i::alt, gps, GpsFixValid, ins_int, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ltp_def_from_lla_i(), stateGetLlaOrigin_i(), and stateSetLocalOrigin_i().
Referenced by reset_cb().
|
static |
|
static |
|
static |
|
static |
Definition at line 597 of file ins_int.c.
References float_quat_vmult(), hff_update_vel(), ins_int, ins_ned_to_state(), POS_BFP_OF_REAL, QUAT_INVERT, SPEED_BFP_OF_REAL, stateGetNedToBodyQuat_f(), vff_update_vz_conf(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by ins_int_init().
| struct InsInt ins_int |
global INS state
Definition at line 166 of file ins_int.c.
Referenced by baro_cb(), ins_int_init(), ins_int_propagate(), ins_int_update_gps(), ins_ned_to_state(), ins_update_from_vff(), pos_est_cb(), reset_ref(), reset_vertical_pos(), reset_vertical_ref(), send_ins(), send_ins_ref(), send_ins_z(), and vel_est_cb().
|
static |
Definition at line 146 of file ins_int.c.
Referenced by ins_int_init().
|
static |
Definition at line 137 of file ins_int.c.
Referenced by ins_int_init().