Daniel Agar
6 years ago
38 changed files with 0 additions and 2210 deletions
@ -1,47 +0,0 @@
@@ -1,47 +0,0 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
############################################################################ |
||||
|
||||
px4_add_module( |
||||
MODULE modules__position_estimator_inav |
||||
MAIN position_estimator_inav |
||||
STACK_MAIN 1200 |
||||
STACK_MAX 4000 |
||||
COMPILE_FLAGS |
||||
SRCS |
||||
position_estimator_inav_main.cpp |
||||
inertial_filter.cpp |
||||
DEPENDS |
||||
terrain_estimation |
||||
git_ecl |
||||
ecl_geo |
||||
) |
||||
@ -1,34 +0,0 @@
@@ -1,34 +0,0 @@
|
||||
/*
|
||||
* inertial_filter.c |
||||
* |
||||
* Copyright (C) 2013 Anton Babushkin. All rights reserved. |
||||
* Author: Anton Babushkin <rk3dov@gmail.com> |
||||
*/ |
||||
|
||||
#include "px4_defines.h" |
||||
#include "inertial_filter.h" |
||||
#include <cmath> |
||||
|
||||
void inertial_filter_predict(float dt, float x[2], float acc) |
||||
{ |
||||
if (PX4_ISFINITE(dt)) { |
||||
if (!PX4_ISFINITE(acc)) { |
||||
acc = 0.0f; |
||||
} |
||||
|
||||
x[0] += x[1] * dt + acc * dt * dt / 2.0f; |
||||
x[1] += acc * dt; |
||||
} |
||||
} |
||||
|
||||
void inertial_filter_correct(float e, float dt, float x[2], int i, float w) |
||||
{ |
||||
if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) { |
||||
float ewdt = e * w * dt; |
||||
x[i] += ewdt; |
||||
|
||||
if (i == 0) { |
||||
x[1] += w * ewdt; |
||||
} |
||||
} |
||||
} |
||||
@ -1,15 +0,0 @@
@@ -1,15 +0,0 @@
|
||||
/*
|
||||
* inertial_filter.h |
||||
* |
||||
* Copyright (C) 2013 Anton Babushkin. All rights reserved. |
||||
* Author: Anton Babushkin <rk3dov@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <stdbool.h> |
||||
#include <drivers/drv_hrt.h> |
||||
|
||||
void inertial_filter_predict(float dt, float x[2], float acc); |
||||
|
||||
void inertial_filter_correct(float e, float dt, float x[2], int i, float w); |
||||
@ -1,346 +0,0 @@
@@ -1,346 +0,0 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/*
|
||||
* @file position_estimator_inav_params.c |
||||
* |
||||
* @author Anton Babushkin <rk3dov@gmail.com> |
||||
* |
||||
* Parameters for position_estimator_inav |
||||
*/ |
||||
|
||||
#include "position_estimator_inav_params.h" |
||||
|
||||
/**
|
||||
* Z axis weight for barometer |
||||
* |
||||
* Weight (cutoff frequency) for barometer altitude measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); |
||||
|
||||
/**
|
||||
* Z axis weight for GPS |
||||
* |
||||
* Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); |
||||
|
||||
/**
|
||||
* Z velocity weight for GPS |
||||
* |
||||
* Weight (cutoff frequency) for GPS altitude velocity measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); |
||||
|
||||
/**
|
||||
* Z axis weight for vision |
||||
* |
||||
* Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f); |
||||
|
||||
/**
|
||||
* Z axis weight for lidar |
||||
* |
||||
* Weight (cutoff frequency) for lidar measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f); |
||||
|
||||
/**
|
||||
* XY axis weight for GPS position |
||||
* |
||||
* Weight (cutoff frequency) for GPS position measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); |
||||
|
||||
/**
|
||||
* XY axis weight for GPS velocity |
||||
* |
||||
* Weight (cutoff frequency) for GPS velocity measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); |
||||
|
||||
/**
|
||||
* XY axis weight for vision position |
||||
* |
||||
* Weight (cutoff frequency) for vision position measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f); |
||||
|
||||
/**
|
||||
* XY axis weight for vision velocity |
||||
* |
||||
* Weight (cutoff frequency) for vision velocity measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); |
||||
|
||||
/**
|
||||
* Weight for mocap system |
||||
* |
||||
* Weight (cutoff frequency) for mocap position measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
|
||||
PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f); |
||||
|
||||
/**
|
||||
* XY axis weight for optical flow |
||||
* |
||||
* Weight (cutoff frequency) for optical flow (velocity) measurements. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 0.8f); |
||||
|
||||
/**
|
||||
* XY axis weight for resetting velocity |
||||
* |
||||
* When velocity sources lost slowly decrease estimated horizontal velocity with this weight. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); |
||||
|
||||
/**
|
||||
* XY axis weight factor for GPS when optical flow available |
||||
* |
||||
* When optical flow data available, multiply GPS weights (for position and velocity) by this factor. |
||||
* |
||||
* @min 0.0 |
||||
* @max 1.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); |
||||
|
||||
/**
|
||||
* Accelerometer bias estimation weight |
||||
* |
||||
* Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. |
||||
* |
||||
* @min 0.0 |
||||
* @max 0.1 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); |
||||
|
||||
/**
|
||||
* Optical flow scale factor |
||||
* |
||||
* Factor to scale optical flow |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.35f); |
||||
|
||||
/**
|
||||
* Minimal acceptable optical flow quality |
||||
* |
||||
* 0 - lowest quality, 1 - best quality. |
||||
* |
||||
* @min 0.0 |
||||
* @max 1.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f); |
||||
|
||||
/**
|
||||
* Sonar maximal error for new surface |
||||
* |
||||
* If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). |
||||
* |
||||
* @min 0.0 |
||||
* @max 1.0 |
||||
* @unit m |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.2f); |
||||
|
||||
/**
|
||||
* Land detector time |
||||
* |
||||
* Vehicle assumed landed if no altitude changes happened during this time on low throttle. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @unit s |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); |
||||
|
||||
/**
|
||||
* Land detector altitude dispersion threshold |
||||
* |
||||
* Dispersion threshold for triggering land detector. |
||||
* |
||||
* @min 0.0 |
||||
* @max 10.0 |
||||
* @unit m |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); |
||||
|
||||
/**
|
||||
* Land detector throttle threshold |
||||
* |
||||
* Value should be lower than minimal hovering thrust. Half of it is good choice. |
||||
* |
||||
* @min 0.0 |
||||
* @max 1.0 |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); |
||||
|
||||
/**
|
||||
* GPS delay |
||||
* |
||||
* GPS delay compensation |
||||
* |
||||
* @min 0.0 |
||||
* @max 1.0 |
||||
* @unit s |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); |
||||
|
||||
/**
|
||||
* Flow module offset (center of rotation) in X direction |
||||
* |
||||
* Yaw X flow compensation |
||||
* |
||||
* @min -1.0 |
||||
* @max 1.0 |
||||
* @unit m |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); |
||||
|
||||
/**
|
||||
* Flow module offset (center of rotation) in Y direction |
||||
* |
||||
* Yaw Y flow compensation |
||||
* |
||||
* @min -1.0 |
||||
* @max 1.0 |
||||
* @unit m |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); |
||||
|
||||
/**
|
||||
* Mo-cap |
||||
* |
||||
* Set to 0 if using fake GPS |
||||
* |
||||
* @value 0 Mo-cap enabled |
||||
* @value 1 Mo-cap disabled |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0); |
||||
|
||||
/**
|
||||
* LIDAR for altitude estimation |
||||
* |
||||
* @boolean |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_LIDAR_EST, 0); |
||||
|
||||
/**
|
||||
* LIDAR calibration offset |
||||
* |
||||
* LIDAR calibration offset. Value will be added to the measured distance |
||||
* |
||||
* @min -20 |
||||
* @max 20 |
||||
* @unit m |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(INAV_LIDAR_OFF, 0.0f); |
||||
|
||||
/**
|
||||
* Disable vision input |
||||
* |
||||
* Set to the appropriate key (328754) to disable vision input. |
||||
* |
||||
* @reboot_required true |
||||
* @min 0 |
||||
* @max 328754 |
||||
* @category Developer |
||||
* @group Position Estimator INAV |
||||
*/ |
||||
PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); |
||||
File diff suppressed because it is too large
Load Diff
@ -1,121 +0,0 @@
@@ -1,121 +0,0 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/*
|
||||
* @file position_estimator_inav_params.c |
||||
* |
||||
* @author Anton Babushkin <rk3dov@gmail.com> |
||||
* |
||||
* Parameters definition for position_estimator_inav |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <parameters/param.h> |
||||
|
||||
struct position_estimator_inav_params { |
||||
float w_z_baro; |
||||
float w_z_gps_p; |
||||
float w_z_gps_v; |
||||
float w_z_vision_p; |
||||
float w_z_lidar; |
||||
float w_xy_gps_p; |
||||
float w_xy_gps_v; |
||||
float w_xy_vision_p; |
||||
float w_xy_vision_v; |
||||
float w_mocap_p; |
||||
float w_xy_flow; |
||||
float w_xy_res_v; |
||||
float w_gps_flow; |
||||
float w_acc_bias; |
||||
float flow_k; |
||||
float flow_q_min; |
||||
float lidar_err; |
||||
float land_t; |
||||
float land_disp; |
||||
float land_thr; |
||||
int32_t no_vision; |
||||
float delay_gps; |
||||
float flow_module_offset_x; |
||||
float flow_module_offset_y; |
||||
int32_t disable_mocap; |
||||
int32_t enable_lidar_alt_est; |
||||
float lidar_calibration_offset; |
||||
int32_t att_ext_hdg_m; |
||||
}; |
||||
|
||||
struct position_estimator_inav_param_handles { |
||||
param_t w_z_baro; |
||||
param_t w_z_gps_p; |
||||
param_t w_z_gps_v; |
||||
param_t w_z_vision_p; |
||||
param_t w_z_lidar; |
||||
param_t w_xy_gps_p; |
||||
param_t w_xy_gps_v; |
||||
param_t w_xy_vision_p; |
||||
param_t w_xy_vision_v; |
||||
param_t w_mocap_p; |
||||
param_t w_xy_flow; |
||||
param_t w_xy_res_v; |
||||
param_t w_gps_flow; |
||||
param_t w_acc_bias; |
||||
param_t flow_k; |
||||
param_t flow_q_min; |
||||
param_t lidar_err; |
||||
param_t land_t; |
||||
param_t land_disp; |
||||
param_t land_thr; |
||||
param_t no_vision; |
||||
param_t delay_gps; |
||||
param_t flow_module_offset_x; |
||||
param_t flow_module_offset_y; |
||||
param_t disable_mocap; |
||||
param_t enable_lidar_alt_est; |
||||
param_t lidar_calibration_offset; |
||||
param_t att_ext_hdg_m; |
||||
}; |
||||
|
||||
#define CBRK_NO_VISION_KEY 328754 |
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values |
||||
* |
||||
*/ |
||||
int inav_parameters_init(struct position_estimator_inav_param_handles *h); |
||||
|
||||
/**
|
||||
* Update all parameters |
||||
* |
||||
*/ |
||||
int inav_parameters_update(const struct position_estimator_inav_param_handles *h, |
||||
struct position_estimator_inav_params *p); |
||||
Loading…
Reference in new issue