mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Added local position estimator.
This commit is contained in:
jgoppert
Lorenz Meier
parent
faa80a51e8
commit
0106be3e89
@ -236,6 +236,19 @@ add_custom_target(submodule_clean
|
||||
COMMAND rm -rf .git/modules/*
|
||||
)
|
||||
|
||||
#=============================================================================
|
||||
# misc targets
|
||||
#
|
||||
add_custom_target(check_format
|
||||
COMMAND Tools/check_code_style.sh
|
||||
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
|
||||
)
|
||||
|
||||
add_custom_target(config
|
||||
COMMAND cmake-gui .
|
||||
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
|
||||
)
|
||||
|
||||
#=============================================================================
|
||||
# external libraries
|
||||
#
|
||||
|
||||
52
Makefile
52
Makefile
@ -113,12 +113,18 @@ px4fmu-v2_default:
|
||||
px4fmu-v2_simple:
|
||||
$(call cmake-build,nuttx_px4fmu-v2_simple)
|
||||
|
||||
px4fmu-v2_lpe:
|
||||
$(call cmake-build,nuttx_px4fmu-v2_lpe)
|
||||
|
||||
nuttx_sim_simple:
|
||||
$(call cmake-build,$@)
|
||||
|
||||
posix_sitl_simple:
|
||||
$(call cmake-build,$@)
|
||||
|
||||
posix_sitl_lpe:
|
||||
$(call cmake-build,$@)
|
||||
|
||||
ros_sitl_simple:
|
||||
$(call cmake-build,$@)
|
||||
|
||||
@ -134,44 +140,13 @@ posix_sitl_default: posix_sitl_simple
|
||||
|
||||
ros: ros_sitl_simple
|
||||
|
||||
run_sitl_quad: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rcS none jmavsim
|
||||
sitl_deprecation:
|
||||
@echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead."
|
||||
@echo "Change init script with 'make posix_sitl_default config'"
|
||||
|
||||
run_sitl_iris: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rcS_iris_gazebo none gazebo
|
||||
|
||||
run_sitl_plane: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing
|
||||
|
||||
run_sitl_ros: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros
|
||||
|
||||
lldb_sitl_quad: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rcS lldb jmavsim
|
||||
|
||||
lldb_sitl_plane: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb
|
||||
|
||||
lldb_sitl_ros: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb
|
||||
|
||||
gdb_sitl_quad: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rcS gdb jmavsim
|
||||
|
||||
gdb_sitl_plane: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb
|
||||
|
||||
gdb_sitl_ros: posix
|
||||
Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb
|
||||
|
||||
sitl_quad:
|
||||
@echo "Deprecated. Use 'run_sitl_quad' instead."
|
||||
|
||||
sitl_plane:
|
||||
@echo "Deprecated. Use 'run_sitl_plane' instead."
|
||||
|
||||
sitl_ros:
|
||||
@echo "Deprecated. Use 'run_sitl_ros' instead."
|
||||
sitl_quad: sitl_deprecation
|
||||
sitl_plane: sitl_deprecation
|
||||
sitl_ros: sitl_deprecation
|
||||
|
||||
# Other targets
|
||||
# --------------------------------------------------------------------
|
||||
@ -184,7 +159,8 @@ clean:
|
||||
@(cd src/modules/uavcan/libuavcan && git clean -d -f -x)
|
||||
|
||||
# targets handled by cmake
|
||||
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan
|
||||
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
|
||||
run_sitl config
|
||||
$(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
|
||||
|
||||
.PHONY: clean
|
||||
|
||||
@ -12,7 +12,13 @@ then
|
||||
attitude_estimator_q start
|
||||
position_estimator_inav start
|
||||
else
|
||||
ekf_att_pos_estimator start
|
||||
if param compare LPE_ENABLED 1
|
||||
then
|
||||
attitude_estimator_q start
|
||||
local_position_estimator start
|
||||
else
|
||||
ekf_att_pos_estimator start
|
||||
fi
|
||||
fi
|
||||
|
||||
if mc_att_control start
|
||||
|
||||
@ -26,6 +26,7 @@ for fn in $(find src/examples \
|
||||
src/modules/dataman \
|
||||
src/modules/fixedwing_backside \
|
||||
src/modules/segway \
|
||||
src/modules/local_position_estimator \
|
||||
src/modules/unit_test \
|
||||
src/modules/systemlib \
|
||||
src/modules/controllib \
|
||||
|
||||
@ -1,2 +1,2 @@
|
||||
handle SIGCONT nostop
|
||||
handle SIGCONT nostop noprint nopass
|
||||
run
|
||||
|
||||
@ -1,10 +1,28 @@
|
||||
#!/bin/bash
|
||||
cp Tools/posix_lldbinit build_posix_sitl_simple/src/firmware/posix/.lldbinit
|
||||
cp Tools/posix.gdbinit build_posix_sitl_simple/src/firmware/posix/.gdbinit
|
||||
|
||||
rc_script=$1
|
||||
debugger=$2
|
||||
program=$3
|
||||
build_path=$4
|
||||
|
||||
echo SITL ARGS
|
||||
echo rc_script: $rc_script
|
||||
echo debugger: $debugger
|
||||
echo program: $program
|
||||
echo build_path: $buid_path
|
||||
|
||||
if [ "$#" != 4 ]
|
||||
then
|
||||
echo usage: sitl_run.sh rc_script debugger program build_path
|
||||
exit 1
|
||||
fi
|
||||
|
||||
cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit
|
||||
cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
|
||||
|
||||
SIM_PID=0
|
||||
|
||||
if [ "$3" == "jmavsim" ]
|
||||
if [ "$program" == "jmavsim" ]
|
||||
then
|
||||
cd Tools/jMAVSim
|
||||
ant
|
||||
@ -22,19 +40,19 @@ then
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
cd build_posix_sitl_simple/src/firmware/posix
|
||||
cd $build_path/src/firmware/posix
|
||||
mkdir -p rootfs/fs/microsd
|
||||
mkdir -p rootfs/eeprom
|
||||
touch rootfs/eeprom/parameters
|
||||
# Start Java simulator
|
||||
if [ "$2" == "lldb" ]
|
||||
if [ "$debugger" == "lldb" ]
|
||||
then
|
||||
lldb -- mainapp ../../../../$1
|
||||
elif [ "$2" == "gdb" ]
|
||||
lldb -- mainapp ../../../../$rc_script
|
||||
elif [ "$debugger" == "gdb" ]
|
||||
then
|
||||
gdb --args mainapp ../../../../$1
|
||||
gdb --args mainapp ../../../../$rc_script
|
||||
else
|
||||
./mainapp ../../../../$1
|
||||
./mainapp ../../../../$rc_script
|
||||
fi
|
||||
|
||||
if [ "$3" == "jmavsim" ]
|
||||
|
||||
5
cmake/configs/nuttx_px4fmu-v2_lpe.cmake
Normal file
5
cmake/configs/nuttx_px4fmu-v2_lpe.cmake
Normal file
@ -0,0 +1,5 @@
|
||||
include(cmake/configs/nuttx_px4fmu-v2_default.cmake)
|
||||
|
||||
list(APPEND config_module_list
|
||||
modules/local_position_estimator
|
||||
)
|
||||
9
cmake/configs/posix_sitl_lpe.cmake
Normal file
9
cmake/configs/posix_sitl_lpe.cmake
Normal file
@ -0,0 +1,9 @@
|
||||
include(cmake/configs/posix_sitl_simple.cmake)
|
||||
|
||||
list(APPEND config_module_list
|
||||
modules/local_position_estimator
|
||||
)
|
||||
|
||||
set(config_sitl_rcS
|
||||
posix-configs/SITL/init/rcS_lpe
|
||||
)
|
||||
@ -62,6 +62,27 @@ set(config_extra_builtin_cmds
|
||||
sercon
|
||||
)
|
||||
|
||||
set(config_sitl_rcS
|
||||
posix-configs/SITL/init/rcS
|
||||
CACHE FILEPATH "init script for sitl"
|
||||
)
|
||||
|
||||
set(config_sitl_viewer
|
||||
jmavsim
|
||||
CACHE STRING "viewer for sitl"
|
||||
)
|
||||
set_property(CACHE config_sitl_viewer
|
||||
PROPERTY STRINGS "jmavsim;none")
|
||||
|
||||
set(config_sitl_debugger
|
||||
disable
|
||||
CACHE STRING "debugger for sitl"
|
||||
)
|
||||
set_property(CACHE config_sitl_debugger
|
||||
PROPERTY STRINGS "disable;gdb;lldb")
|
||||
|
||||
|
||||
|
||||
add_custom_target(sercon)
|
||||
set_target_properties(sercon PROPERTIES
|
||||
MAIN "sercon" STACK "2048")
|
||||
|
||||
65
posix-configs/SITL/init/rcS_lpe
Normal file
65
posix-configs/SITL/init/rcS_lpe
Normal file
@ -0,0 +1,65 @@
|
||||
uorb start
|
||||
simulator start -s
|
||||
param load
|
||||
param set MAV_TYPE 2
|
||||
param set MC_PITCHRATE_P 0.15
|
||||
param set MC_ROLLRATE_P 0.15
|
||||
param set MC_YAW_P 2.0
|
||||
param set MC_YAWRATE_P 0.35
|
||||
param set SYS_AUTOSTART 4010
|
||||
param set SYS_RESTART_TYPE 2
|
||||
dataman start
|
||||
param set CAL_GYRO0_ID 2293760
|
||||
param set CAL_ACC0_ID 1376256
|
||||
param set CAL_ACC1_ID 1310720
|
||||
param set CAL_MAG0_ID 196608
|
||||
param set CAL_GYRO0_XOFF 0.01
|
||||
param set CAL_ACC0_XOFF 0.01
|
||||
param set CAL_ACC0_YOFF -0.01
|
||||
param set CAL_ACC0_ZOFF 0.01
|
||||
param set CAL_ACC0_XSCALE 1.01
|
||||
param set CAL_ACC0_YSCALE 1.01
|
||||
param set CAL_ACC0_ZSCALE 1.01
|
||||
param set CAL_ACC1_XOFF 0.01
|
||||
param set CAL_MAG0_XOFF 0.01
|
||||
param set MPC_XY_P 0.4
|
||||
param set MPC_XY_VEL_P 0.2
|
||||
param set MPC_XY_VEL_D 0.005
|
||||
param set SENS_BOARD_ROT 0
|
||||
|
||||
# changes for LPE
|
||||
param set COM_RC_IN_MODE 1
|
||||
param set LPE_BETA_MAX 10000
|
||||
|
||||
rgbled start
|
||||
tone_alarm start
|
||||
gyrosim start
|
||||
accelsim start
|
||||
barosim start
|
||||
adcsim start
|
||||
gpssim start
|
||||
pwm_out_sim mode_pwm
|
||||
sleep 1
|
||||
sensors start
|
||||
commander start
|
||||
land_detector start multicopter
|
||||
navigator start
|
||||
attitude_estimator_q start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
|
||||
mavlink start -u 14556 -r 2000000
|
||||
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
|
||||
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
|
||||
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
|
||||
mavlink stream -r 80 -s ATTITUDE -u 14556
|
||||
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
|
||||
mavlink stream -r 20 -s RC_CHANNELS -u 14556
|
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
|
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
|
||||
mavlink boot_complete
|
||||
sdlog2 start -r 100 -e -t -a
|
||||
|
||||
# start LPE at end, when we know it is ok to init sensors
|
||||
sleep 5
|
||||
local_position_estimator start
|
||||
@ -5,13 +5,6 @@ px4_nuttx_generate_builtin_commands(
|
||||
${config_extra_builtin_cmds}
|
||||
)
|
||||
|
||||
px4_nuttx_add_romfs(OUT romfs
|
||||
ROOT ROMFS/px4fmu_common
|
||||
EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin
|
||||
)
|
||||
|
||||
add_dependencies(romfs fw_io)
|
||||
|
||||
# add executable
|
||||
add_executable(firmware_nuttx
|
||||
builtin_commands.c)
|
||||
@ -33,8 +26,6 @@ if(NOT ${BOARD} STREQUAL "sim")
|
||||
set_target_properties(firmware_nuttx PROPERTIES LINK_FLAGS ${main_link_flags})
|
||||
endif()
|
||||
|
||||
set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${target_name}.px4)
|
||||
|
||||
target_link_libraries(firmware_nuttx
|
||||
-Wl,--start-group
|
||||
${module_libraries}
|
||||
@ -49,6 +40,14 @@ add_custom_target(check_weak
|
||||
)
|
||||
|
||||
if(NOT ${BOARD} STREQUAL "sim")
|
||||
|
||||
|
||||
px4_nuttx_add_romfs(OUT romfs
|
||||
ROOT ROMFS/px4fmu_common
|
||||
EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin
|
||||
)
|
||||
add_dependencies(romfs fw_io)
|
||||
|
||||
set(fw_file
|
||||
${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4)
|
||||
|
||||
|
||||
@ -24,4 +24,11 @@ else()
|
||||
)
|
||||
endif()
|
||||
|
||||
add_custom_target(run_sitl
|
||||
COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}"
|
||||
"${config_sitl_viewer}" "${CMAKE_BINARY_DIR}"
|
||||
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
|
||||
)
|
||||
add_dependencies(run_sitl mainapp)
|
||||
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
|
||||
@ -539,6 +539,7 @@ int blockRandGaussTest()
|
||||
}
|
||||
|
||||
float stdDev = sqrt(sum / (n - 1));
|
||||
(void)(stdDev);
|
||||
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
|
||||
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
|
||||
printf("PASS\n");
|
||||
|
||||
1379
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
Normal file
1379
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
Normal file
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,310 @@
|
||||
#pragma once
|
||||
|
||||
#include <controllib/uorb/blocks.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#ifdef USE_MATRIX_LIB
|
||||
#include "matrix/src/Matrix.hpp"
|
||||
using namespace matrix;
|
||||
#else
|
||||
#include <Eigen/Eigen>
|
||||
using namespace Eigen;
|
||||
#endif
|
||||
|
||||
// uORB Subscriptions
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vision_position_estimate.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
|
||||
// uORB Publications
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/filtered_bottom_flow.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
|
||||
#define CBRK_NO_VISION_KEY 328754
|
||||
|
||||
using namespace control;
|
||||
|
||||
enum fault_t {
|
||||
FAULT_NONE = 0,
|
||||
FAULT_MINOR,
|
||||
FAULT_SEVERE
|
||||
};
|
||||
|
||||
enum sensor_t {
|
||||
SENSOR_BARO = 0,
|
||||
SENSOR_GPS,
|
||||
SENSOR_LIDAR,
|
||||
SENSOR_FLOW,
|
||||
SENSOR_SONAR,
|
||||
SENSOR_VISION,
|
||||
SENSOR_MOCAP
|
||||
};
|
||||
|
||||
class BlockLocalPositionEstimator : public control::SuperBlock
|
||||
{
|
||||
//
|
||||
// The purpose of this estimator is to provide a robust solution for
|
||||
// indoor flight.
|
||||
//
|
||||
// dynamics:
|
||||
//
|
||||
// x(+) = A * x(-) + B * u(+)
|
||||
// y_i = C_i*x
|
||||
//
|
||||
// kalman filter
|
||||
//
|
||||
// E[xx'] = P
|
||||
// E[uu'] = W
|
||||
// E[y_iy_i'] = R_i
|
||||
//
|
||||
// prediction
|
||||
// x(+|-) = A*x(-|-) + B*u(+)
|
||||
// P(+|-) = A*P(-|-)*A' + B*W*B'
|
||||
//
|
||||
// correction
|
||||
// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) )
|
||||
//
|
||||
//
|
||||
// input:
|
||||
// ax, ay, az (acceleration NED)
|
||||
//
|
||||
// states:
|
||||
// px, py, pz , ( position NED)
|
||||
// vx, vy, vz ( vel NED),
|
||||
// bx, by, bz ( TODO accelerometer bias)
|
||||
// tz (TODO terrain altitude)
|
||||
//
|
||||
// measurements:
|
||||
//
|
||||
// sonar: pz (measured d*cos(phi)*cos(theta))
|
||||
//
|
||||
// baro: pz
|
||||
//
|
||||
// flow: vx, vy (flow is in body x, y frame)
|
||||
//
|
||||
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
|
||||
//
|
||||
// lidar: px (actual measured d*cos(phi)*cos(theta))
|
||||
//
|
||||
// vision: px, py, pz, vx, vy, vz
|
||||
//
|
||||
// mocap: px, py, pz
|
||||
//
|
||||
public:
|
||||
BlockLocalPositionEstimator();
|
||||
void update();
|
||||
virtual ~BlockLocalPositionEstimator();
|
||||
|
||||
private:
|
||||
// prevent copy and assignment
|
||||
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);
|
||||
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);
|
||||
|
||||
// constants
|
||||
static const uint8_t n_x = 9;
|
||||
static const uint8_t n_u = 3; // 3 accelerations
|
||||
static const uint8_t n_y_flow = 2;
|
||||
static const uint8_t n_y_sonar = 1;
|
||||
static const uint8_t n_y_baro = 1;
|
||||
static const uint8_t n_y_lidar = 1;
|
||||
static const uint8_t n_y_gps = 6;
|
||||
static const uint8_t n_y_vision = 3;
|
||||
static const uint8_t n_y_mocap = 3;
|
||||
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz};
|
||||
enum {U_ax = 0, U_ay, U_az};
|
||||
enum {Y_baro_z = 0};
|
||||
enum {Y_lidar_z = 0};
|
||||
enum {Y_flow_x = 0, Y_flow_y};
|
||||
enum {Y_sonar_z = 0};
|
||||
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz};
|
||||
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz};
|
||||
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z};
|
||||
enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM};
|
||||
|
||||
// methods
|
||||
// ----------------------------
|
||||
void initP();
|
||||
|
||||
// predict the next state
|
||||
void predict();
|
||||
|
||||
// correct the state prediction wtih a measurement
|
||||
void correctBaro();
|
||||
void correctGps();
|
||||
void correctLidar();
|
||||
void correctFlow();
|
||||
void correctSonar();
|
||||
void correctVision();
|
||||
void correctmocap();
|
||||
|
||||
// sensor initialization
|
||||
void updateHome();
|
||||
void initBaro();
|
||||
void initGps();
|
||||
void initLidar();
|
||||
void initSonar();
|
||||
void initFlow();
|
||||
void initVision();
|
||||
void initmocap();
|
||||
|
||||
// publications
|
||||
void publishLocalPos();
|
||||
void publishGlobalPos();
|
||||
void publishFilteredFlow();
|
||||
void publishEstimatorStatus();
|
||||
|
||||
// attributes
|
||||
// ----------------------------
|
||||
|
||||
// subscriptions
|
||||
uORB::Subscription<vehicle_status_s> _sub_status;
|
||||
uORB::Subscription<actuator_armed_s> _sub_armed;
|
||||
uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
|
||||
uORB::Subscription<vehicle_attitude_s> _sub_att;
|
||||
uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
|
||||
uORB::Subscription<optical_flow_s> _sub_flow;
|
||||
uORB::Subscription<sensor_combined_s> _sub_sensor;
|
||||
uORB::Subscription<distance_sensor_s> _sub_distance;
|
||||
uORB::Subscription<parameter_update_s> _sub_param_update;
|
||||
uORB::Subscription<manual_control_setpoint_s> _sub_manual;
|
||||
uORB::Subscription<home_position_s> _sub_home;
|
||||
uORB::Subscription<vehicle_gps_position_s> _sub_gps;
|
||||
uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
|
||||
uORB::Subscription<att_pos_mocap_s> _sub_mocap;
|
||||
|
||||
// publications
|
||||
uORB::Publication<vehicle_local_position_s> _pub_lpos;
|
||||
uORB::Publication<vehicle_global_position_s> _pub_gpos;
|
||||
uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
|
||||
uORB::Publication<estimator_status_s> _pub_est_status;
|
||||
|
||||
// map projection
|
||||
struct map_projection_reference_s _map_ref;
|
||||
|
||||
// parameters
|
||||
BlockParamInt _integrate;
|
||||
|
||||
BlockParamFloat _flow_xy_stddev;
|
||||
BlockParamFloat _sonar_z_stddev;
|
||||
|
||||
BlockParamFloat _lidar_z_stddev;
|
||||
|
||||
BlockParamFloat _accel_xy_stddev;
|
||||
BlockParamFloat _accel_z_stddev;
|
||||
|
||||
BlockParamFloat _baro_stddev;
|
||||
|
||||
BlockParamFloat _gps_xy_stddev;
|
||||
BlockParamFloat _gps_z_stddev;
|
||||
|
||||
BlockParamFloat _gps_vxy_stddev;
|
||||
BlockParamFloat _gps_vz_stddev;
|
||||
|
||||
BlockParamFloat _gps_eph_max;
|
||||
|
||||
BlockParamFloat _vision_xy_stddev;
|
||||
BlockParamFloat _vision_z_stddev;
|
||||
BlockParamInt _no_vision;
|
||||
BlockParamFloat _beta_max;
|
||||
|
||||
BlockParamFloat _mocap_p_stddev;
|
||||
|
||||
// process noise
|
||||
BlockParamFloat _pn_p_noise_power;
|
||||
BlockParamFloat _pn_v_noise_power;
|
||||
BlockParamFloat _pn_b_noise_power;
|
||||
|
||||
// misc
|
||||
struct pollfd _polls[3];
|
||||
uint64_t _timeStamp;
|
||||
uint64_t _time_last_xy;
|
||||
uint64_t _time_last_flow;
|
||||
uint64_t _time_last_baro;
|
||||
uint64_t _time_last_gps;
|
||||
uint64_t _time_last_lidar;
|
||||
uint64_t _time_last_sonar;
|
||||
uint64_t _time_last_vision_p;
|
||||
uint64_t _time_last_mocap;
|
||||
int _mavlink_fd;
|
||||
|
||||
// initialization flags
|
||||
bool _baroInitialized;
|
||||
bool _gpsInitialized;
|
||||
bool _lidarInitialized;
|
||||
bool _sonarInitialized;
|
||||
bool _flowInitialized;
|
||||
bool _visionInitialized;
|
||||
bool _mocapInitialized;
|
||||
|
||||
// init counts
|
||||
int _baroInitCount;
|
||||
int _gpsInitCount;
|
||||
int _lidarInitCount;
|
||||
int _sonarInitCount;
|
||||
int _flowInitCount;
|
||||
int _visionInitCount;
|
||||
int _mocapInitCount;
|
||||
|
||||
// reference altitudes
|
||||
float _altHome;
|
||||
bool _altHomeInitialized;
|
||||
float _baroAltHome;
|
||||
float _gpsAltHome;
|
||||
float _lidarAltHome;
|
||||
float _sonarAltHome;
|
||||
float _flowAltHome;
|
||||
Vector3f _visionHome;
|
||||
Vector3f _mocapHome;
|
||||
|
||||
// flow integration
|
||||
float _flowX;
|
||||
float _flowY;
|
||||
float _flowMeanQual;
|
||||
|
||||
// referene lat/lon
|
||||
double _gpsLatHome;
|
||||
double _gpsLonHome;
|
||||
|
||||
// status
|
||||
bool _canEstimateXY;
|
||||
bool _canEstimateZ;
|
||||
bool _xyTimeout;
|
||||
|
||||
// sensor faults
|
||||
fault_t _baroFault;
|
||||
fault_t _gpsFault;
|
||||
fault_t _lidarFault;
|
||||
fault_t _flowFault;
|
||||
fault_t _sonarFault;
|
||||
fault_t _visionFault;
|
||||
fault_t _mocapFault;
|
||||
|
||||
bool _visionTimeout;
|
||||
bool _mocapTimeout;
|
||||
|
||||
perf_counter_t _loop_perf;
|
||||
perf_counter_t _interval_perf;
|
||||
perf_counter_t _err_perf;
|
||||
|
||||
// state space
|
||||
Matrix<float, n_x, 1> _x; // state vector
|
||||
Matrix<float, n_u, 1> _u; // input vector
|
||||
Matrix<float, n_x, n_x> _P; // state covariance matrix
|
||||
};
|
||||
57
src/modules/local_position_estimator/CMakeLists.txt
Normal file
57
src/modules/local_position_estimator/CMakeLists.txt
Normal file
@ -0,0 +1,57 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
if(${OS} STREQUAL "nuttx")
|
||||
list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
|
||||
elseif(${OS} STREQUAL "posix")
|
||||
list(APPEND MODULE_CFLAGS -Wno-error)
|
||||
# add matrix tests
|
||||
add_subdirectory(matrix)
|
||||
endif()
|
||||
|
||||
# use custom matrix lib instead of Eigen
|
||||
add_definitions(-DUSE_MATRIX_LIB)
|
||||
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__local_position_estimator
|
||||
MAIN local_position_estimator
|
||||
STACK 9216
|
||||
COMPILE_FLAGS ${MODULE_CFLAGS}
|
||||
SRCS
|
||||
local_position_estimator_main.cpp
|
||||
BlockLocalPositionEstimator.cpp
|
||||
params.c
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
@ -0,0 +1,169 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file local_position_estimator.cpp
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
* @author Mohammed Kabir
|
||||
* @author Nuno Marques <n.marques21@hotmail.com>
|
||||
*
|
||||
* Local position estimator
|
||||
*/
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
#include <px4_posix.h>
|
||||
|
||||
#include "BlockLocalPositionEstimator.hpp"
|
||||
|
||||
static volatile bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static volatile bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int local_position_estimator_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static int usage(const char *reason);
|
||||
|
||||
static int
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
}
|
||||
|
||||
fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p <additional params>]\n\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int local_position_estimator_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1) {
|
||||
usage("missing command");
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
warnx("already running");
|
||||
/* this is not an error */
|
||||
return 0;
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
|
||||
deamon_task = px4_task_spawn_cmd("lp_estimator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
10240,
|
||||
local_position_estimator_thread_main,
|
||||
(argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
if (thread_running) {
|
||||
warnx("stop");
|
||||
thread_should_exit = true;
|
||||
|
||||
} else {
|
||||
warnx("not started");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
warnx("is running");
|
||||
|
||||
} else {
|
||||
warnx("not started");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
return 1;
|
||||
}
|
||||
|
||||
int local_position_estimator_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
warnx("starting");
|
||||
|
||||
using namespace control;
|
||||
|
||||
|
||||
thread_running = true;
|
||||
|
||||
BlockLocalPositionEstimator est;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
est.update();
|
||||
}
|
||||
|
||||
warnx("exiting.");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
1
src/modules/local_position_estimator/matrix/.gitignore
vendored
Normal file
1
src/modules/local_position_estimator/matrix/.gitignore
vendored
Normal file
@ -0,0 +1 @@
|
||||
build*/
|
||||
13
src/modules/local_position_estimator/matrix/CMakeLists.txt
Normal file
13
src/modules/local_position_estimator/matrix/CMakeLists.txt
Normal file
@ -0,0 +1,13 @@
|
||||
cmake_minimum_required(VERSION 2.8)
|
||||
|
||||
project(matrix CXX)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "RelWithDebInfo")
|
||||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
|
||||
|
||||
enable_testing()
|
||||
|
||||
include_directories(src)
|
||||
|
||||
add_subdirectory(test)
|
||||
459
src/modules/local_position_estimator/matrix/src/Matrix.hpp
Normal file
459
src/modules/local_position_estimator/matrix/src/Matrix.hpp
Normal file
@ -0,0 +1,459 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template<typename T, size_t M, size_t N>
|
||||
class Matrix
|
||||
{
|
||||
|
||||
private:
|
||||
T _data[M][N];
|
||||
size_t _rows;
|
||||
size_t _cols;
|
||||
|
||||
public:
|
||||
|
||||
Matrix() :
|
||||
_data(),
|
||||
_rows(M),
|
||||
_cols(N)
|
||||
{
|
||||
}
|
||||
|
||||
Matrix(const T *data) :
|
||||
_data(),
|
||||
_rows(M),
|
||||
_cols(N)
|
||||
{
|
||||
memcpy(_data, data, sizeof(_data));
|
||||
}
|
||||
|
||||
Matrix(const Matrix &other) :
|
||||
_data(),
|
||||
_rows(M),
|
||||
_cols(N)
|
||||
{
|
||||
memcpy(_data, other._data, sizeof(_data));
|
||||
}
|
||||
|
||||
Matrix(T x, T y, T z) :
|
||||
_data(),
|
||||
_rows(M),
|
||||
_cols(N)
|
||||
{
|
||||
// TODO, limit to only 3x1 matrices
|
||||
_data[0][0] = x;
|
||||
_data[1][0] = y;
|
||||
_data[2][0] = z;
|
||||
}
|
||||
|
||||
/**
|
||||
* Accessors/ Assignment etc.
|
||||
*/
|
||||
|
||||
inline size_t rows() const
|
||||
{
|
||||
return _rows;
|
||||
}
|
||||
|
||||
inline size_t cols() const
|
||||
{
|
||||
return _rows;
|
||||
}
|
||||
|
||||
inline T operator()(size_t i) const
|
||||
{
|
||||
return _data[i][0];
|
||||
}
|
||||
|
||||
inline T operator()(size_t i, size_t j) const
|
||||
{
|
||||
return _data[i][j];
|
||||
}
|
||||
|
||||
inline T &operator()(size_t i)
|
||||
{
|
||||
return _data[i][0];
|
||||
}
|
||||
|
||||
inline T &operator()(size_t i, size_t j)
|
||||
{
|
||||
return _data[i][j];
|
||||
}
|
||||
|
||||
/**
|
||||
* Matrix Operations
|
||||
*/
|
||||
|
||||
// this might use a lot of programming memory
|
||||
// since it instantiates a class for every
|
||||
// required mult pair, but it provides
|
||||
// compile time size_t checking
|
||||
template<size_t P>
|
||||
Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
|
||||
{
|
||||
const Matrix<T, M, N> &self = *this;
|
||||
Matrix<T, M, P> res;
|
||||
res.setZero();
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t k = 0; k < P; k++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i, k) += self(i, j) * other(j, k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
|
||||
{
|
||||
Matrix<T, M, N> res;
|
||||
const Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) + other(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
bool operator==(const Matrix<T, M, N> &other) const
|
||||
{
|
||||
Matrix<T, M, N> res;
|
||||
const Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
if (self(i , j) != other(i, j)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
|
||||
{
|
||||
Matrix<T, M, N> res;
|
||||
const Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) - other(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void operator+=(const Matrix<T, M, N> &other)
|
||||
{
|
||||
Matrix<T, M, N> &self = *this;
|
||||
self = self + other;
|
||||
}
|
||||
|
||||
void operator-=(const Matrix<T, M, N> &other)
|
||||
{
|
||||
Matrix<T, M, N> &self = *this;
|
||||
self = self - other;
|
||||
}
|
||||
|
||||
void operator*=(const Matrix<T, M, N> &other)
|
||||
{
|
||||
Matrix<T, M, N> &self = *this;
|
||||
self = self * other;
|
||||
}
|
||||
|
||||
/**
|
||||
* Scalar Operations
|
||||
*/
|
||||
|
||||
Matrix<T, M, N> operator*(T scalar) const
|
||||
{
|
||||
Matrix<T, M, N> res;
|
||||
const Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) * scalar;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<T, M, N> operator+(T scalar) const
|
||||
{
|
||||
Matrix<T, M, N> res;
|
||||
Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(i , j) = self(i, j) + scalar;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void operator*=(T scalar)
|
||||
{
|
||||
Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
self(i, j) = self(i, j) * scalar;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void operator/=(T scalar)
|
||||
{
|
||||
Matrix<T, M, N> &self = *this;
|
||||
self = self * (1.0f / scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Misc. Functions
|
||||
*/
|
||||
|
||||
void print()
|
||||
{
|
||||
Matrix<T, M, N> &self = *this;
|
||||
printf("\n");
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
printf("[");
|
||||
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
printf("%10g\t", double(self(i, j)));
|
||||
}
|
||||
|
||||
printf("]\n");
|
||||
}
|
||||
}
|
||||
|
||||
Matrix<T, N, M> transpose() const
|
||||
{
|
||||
Matrix<T, N, M> res;
|
||||
const Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
res(j, i) = self(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<T, M, M> expm(float dt, size_t n) const
|
||||
{
|
||||
Matrix<float, M, M> res;
|
||||
res.setIdentity();
|
||||
Matrix<float, M, M> A_pow = *this;
|
||||
float k_fact = 1;
|
||||
size_t k = 1;
|
||||
|
||||
while (k < n) {
|
||||
res += A_pow * (float(pow(dt, k)) / k_fact);
|
||||
|
||||
if (k == n) { break; }
|
||||
|
||||
A_pow *= A_pow;
|
||||
k_fact *= k;
|
||||
k++;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<T, M, 1> diagonal() const
|
||||
{
|
||||
Matrix<T, M, 1> res;
|
||||
// force square for now
|
||||
const Matrix<T, M, M> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
res(i) = self(i, i);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void setZero()
|
||||
{
|
||||
memset(_data, 0, sizeof(_data));
|
||||
}
|
||||
|
||||
void setIdentity()
|
||||
{
|
||||
setZero();
|
||||
Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < M and i < N; i++) {
|
||||
self(i, i) = 1;
|
||||
}
|
||||
}
|
||||
|
||||
inline void swapRows(size_t a, size_t b)
|
||||
{
|
||||
if (a == b) { return; }
|
||||
|
||||
Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t j = 0; j < cols(); j++) {
|
||||
T tmp = self(a, j);
|
||||
self(a, j) = self(b, j);
|
||||
self(b, j) = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
inline void swapCols(size_t a, size_t b)
|
||||
{
|
||||
if (a == b) { return; }
|
||||
|
||||
Matrix<T, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < rows(); i++) {
|
||||
T tmp = self(i, a);
|
||||
self(i, a) = self(i, b);
|
||||
self(i, b) = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* inverse based on LU factorization with partial pivotting
|
||||
*/
|
||||
Matrix <T, M, M> inverse() const
|
||||
{
|
||||
Matrix<T, M, M> L;
|
||||
L.setIdentity();
|
||||
const Matrix<T, M, M> &A = (*this);
|
||||
Matrix<T, M, M> U = A;
|
||||
Matrix<T, M, M> P;
|
||||
P.setIdentity();
|
||||
|
||||
//printf("A:\n"); A.print();
|
||||
|
||||
// for all diagonal elements
|
||||
for (size_t n = 0; n < N; n++) {
|
||||
|
||||
// if diagonal is zero, swap with row below
|
||||
if (fabsf(U(n, n)) < 1e-8f) {
|
||||
//printf("trying pivot for row %d\n",n);
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (i == n) { continue; }
|
||||
|
||||
//printf("\ttrying row %d\n",i);
|
||||
if (fabsf(U(i, n)) > 1e-8f) {
|
||||
//printf("swapped %d\n",i);
|
||||
U.swapRows(i, n);
|
||||
P.swapRows(i, n);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef MATRIX_ASSERT
|
||||
//printf("A:\n"); A.print();
|
||||
//printf("U:\n"); U.print();
|
||||
//printf("P:\n"); P.print();
|
||||
//fflush(stdout);
|
||||
ASSERT(fabsf(U(n, n)) > 1e-8f);
|
||||
#endif
|
||||
|
||||
// failsafe, return zero matrix
|
||||
if (fabsf(U(n, n)) < 1e-8f) {
|
||||
Matrix<T, M, M> zero;
|
||||
zero.setZero();
|
||||
return zero;
|
||||
}
|
||||
|
||||
// for all rows below diagonal
|
||||
for (size_t i = (n + 1); i < N; i++) {
|
||||
L(i, n) = U(i, n) / U(n, n);
|
||||
|
||||
// add i-th row and n-th row
|
||||
// multiplied by: -a(i,n)/a(n,n)
|
||||
for (size_t k = n; k < N; k++) {
|
||||
U(i, k) -= L(i, n) * U(n, k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//printf("L:\n"); L.print();
|
||||
//printf("U:\n"); U.print();
|
||||
|
||||
// solve LY=P*I for Y by forward subst
|
||||
Matrix<T, M, M> Y = P;
|
||||
|
||||
// for all columns of Y
|
||||
for (size_t c = 0; c < N; c++) {
|
||||
// for all rows of L
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
// for all columns of L
|
||||
for (size_t j = 0; j < i; j++) {
|
||||
// for all existing y
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
Y(i, c) -= L(i, j) * Y(j, c);
|
||||
}
|
||||
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
// Y(i,c) /= L(i,i);
|
||||
// but L(i,i) = 1.0
|
||||
}
|
||||
}
|
||||
|
||||
//printf("Y:\n"); Y.print();
|
||||
|
||||
// solve Ux=y for x by back subst
|
||||
Matrix<T, M, M> X = Y;
|
||||
|
||||
// for all columns of X
|
||||
for (size_t c = 0; c < N; c++) {
|
||||
// for all rows of U
|
||||
for (size_t k = 0; k < N; k++) {
|
||||
// have to go in reverse order
|
||||
size_t i = N - 1 - k;
|
||||
|
||||
// for all columns of U
|
||||
for (size_t j = i + 1; j < N; j++) {
|
||||
// for all existing x
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
X(i, c) -= U(i, j) * X(j, c);
|
||||
}
|
||||
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
X(i, c) /= U(i, i);
|
||||
}
|
||||
}
|
||||
|
||||
//printf("X:\n"); X.print();
|
||||
return X;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
typedef Matrix<float, 2, 1> Vector2f;
|
||||
typedef Matrix<float, 3, 1> Vector3f;
|
||||
typedef Matrix<float, 3, 3> Matrix3f;
|
||||
|
||||
}; // namespace matrix
|
||||
@ -0,0 +1,15 @@
|
||||
set(tests
|
||||
setIdentity
|
||||
inverse
|
||||
matrixMult
|
||||
vectorAssignment
|
||||
matrixAssignment
|
||||
matrixScalarMult
|
||||
transpose
|
||||
)
|
||||
|
||||
foreach(test ${tests})
|
||||
add_executable(${test}
|
||||
${test}.cpp)
|
||||
add_test(${test} ${test})
|
||||
endforeach()
|
||||
30
src/modules/local_position_estimator/matrix/test/inverse.cpp
Normal file
30
src/modules/local_position_estimator/matrix/test/inverse.cpp
Normal file
@ -0,0 +1,30 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
|
||||
Matrix3f A(data);
|
||||
Matrix3f A_I = A.inverse();
|
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
|
||||
Matrix3f A_I_check(data_check);
|
||||
(void)A_I;
|
||||
assert(A_I == A_I_check);
|
||||
|
||||
// stess test
|
||||
static const size_t n = 100;
|
||||
Matrix<float, n, n> A_large;
|
||||
A_large.setIdentity();
|
||||
Matrix<float, n, n> A_large_I;
|
||||
A_large_I.setZero();
|
||||
|
||||
for (size_t i = 0; i < 100; i++) {
|
||||
A_large_I = A_large.inverse();
|
||||
assert(A_large == A_large_I);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,29 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Matrix3f m;
|
||||
m.setZero();
|
||||
m(0, 0) = 1;
|
||||
m(0, 1) = 2;
|
||||
m(0, 2) = 3;
|
||||
m(1, 0) = 4;
|
||||
m(1, 1) = 5;
|
||||
m(1, 2) = 6;
|
||||
m(2, 0) = 7;
|
||||
m(2, 1) = 8;
|
||||
m(2, 2) = 9;
|
||||
|
||||
m.print();
|
||||
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f m2(data);
|
||||
m2.print();
|
||||
|
||||
assert(m == m2);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,26 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
|
||||
Matrix3f A(data);
|
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
|
||||
Matrix3f A_I(data_check);
|
||||
Matrix3f I;
|
||||
I.setIdentity();
|
||||
A.print();
|
||||
A_I.print();
|
||||
Matrix3f R = A * A_I;
|
||||
R.print();
|
||||
assert(R == I);
|
||||
|
||||
Matrix3f R2 = A;
|
||||
R2 *= A_I;
|
||||
R2.print();
|
||||
assert(R2 == I);
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,18 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f A(data);
|
||||
A = A * 2;
|
||||
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
||||
Matrix3f A_check(data_check);
|
||||
A.print();
|
||||
A_check.print();
|
||||
assert(A == A_check);
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,25 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Matrix3f A;
|
||||
A.setIdentity();
|
||||
assert(A.rows() == 3);
|
||||
assert(A.cols() == 3);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
if (i == j) {
|
||||
assert(A(i, j) == 1);
|
||||
|
||||
} else {
|
||||
assert(A(i, j) == 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,18 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 2, 3, 4, 5, 6};
|
||||
Matrix<float, 2, 3> A(data);
|
||||
Matrix<float, 3, 2> A_T = A.transpose();
|
||||
float data_check[9] = {1, 4, 2, 5, 3, 6};
|
||||
Matrix<float, 3, 2> A_T_check(data_check);
|
||||
A_T.print();
|
||||
A_T_check.print();
|
||||
assert(A_T == A_T_check);
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,28 @@
|
||||
#include "Matrix.hpp"
|
||||
#include <assert.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Vector3f v;
|
||||
v(0) = 1;
|
||||
v(1) = 2;
|
||||
v(2) = 3;
|
||||
|
||||
v.print();
|
||||
|
||||
assert(v(0) == 1);
|
||||
assert(v(1) == 2);
|
||||
assert(v(2) == 3);
|
||||
|
||||
Vector3f v2(4, 5, 6);
|
||||
|
||||
v2.print();
|
||||
|
||||
assert(v2(0) == 4);
|
||||
assert(v2(1) == 5);
|
||||
assert(v2(2) == 6);
|
||||
|
||||
return 0;
|
||||
}
|
||||
222
src/modules/local_position_estimator/params.c
Normal file
222
src/modules/local_position_estimator/params.c
Normal file
@ -0,0 +1,222 @@
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
// 16 is max name length
|
||||
|
||||
|
||||
/**
|
||||
* Enable local position estimator.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_ENABLED, 1);
|
||||
|
||||
/**
|
||||
* Enable accelerometer integration for prediction.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_INTEGRATE, 1);
|
||||
|
||||
/**
|
||||
* Optical flow xy standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
|
||||
|
||||
/**
|
||||
* Sonar z standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f);
|
||||
|
||||
/**
|
||||
* Lidar z standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
|
||||
|
||||
/**
|
||||
* Accelerometer xy standard deviation
|
||||
*
|
||||
* Data sheet sqrt(Noise power) = 150ug/sqrt(Hz)
|
||||
* std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2
|
||||
* Since accels sampled at 1000 Hz.
|
||||
*
|
||||
* should be 0.0464
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s^2
|
||||
* @min 0.00001
|
||||
* @max 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
|
||||
|
||||
/**
|
||||
* Accelerometer z standard deviation
|
||||
*
|
||||
* (see Accel x comments)
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s^2
|
||||
* @min 0.00001
|
||||
* @max 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
|
||||
|
||||
/**
|
||||
* Barometric presssure altitude z standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
|
||||
|
||||
/**
|
||||
* GPS xy standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 5
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
|
||||
|
||||
/**
|
||||
* GPS z standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 20
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
|
||||
|
||||
/**
|
||||
* GPS xy velocity standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s
|
||||
* @min 0.01
|
||||
* @max 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f);
|
||||
|
||||
/**
|
||||
* GPS z velocity standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m/s
|
||||
* @min 0.01
|
||||
* @max 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f);
|
||||
|
||||
/**
|
||||
* GPS max eph
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 1.0
|
||||
* @max 5.0
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Vision xy standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
|
||||
|
||||
/**
|
||||
* Vision z standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
|
||||
|
||||
/**
|
||||
* Circuit breaker to disable vision input.
|
||||
*
|
||||
* Set to the appropriate key (328754) to disable vision input.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @min 0
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
|
||||
|
||||
/**
|
||||
* Vicon position standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.01
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
|
||||
|
||||
/**
|
||||
* Position propagation process noise power (variance*sampling rate).
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit (m/s^2)-s
|
||||
* @min 0
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
|
||||
|
||||
/**
|
||||
* Velocity propagation process noise power (variance*sampling rate).
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit (m/s)-s
|
||||
* @min 0
|
||||
* @max 5
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
|
||||
|
||||
/**
|
||||
* Accel bias propagation process noise power (variance*sampling rate).
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit (m/s)-s
|
||||
* @min 0
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f);
|
||||
|
||||
/**
|
||||
* Fault detection threshold, for chi-squared dist.
|
||||
*
|
||||
* TODO add separate params for 1 dof, 3 dof, and 6 dof beta
|
||||
* or false alarm rate in false alarms/hr
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit
|
||||
* @min 3
|
||||
* @max 1000
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f);
|
||||
@ -73,6 +73,7 @@
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/camera_trigger.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <systemlib/err.h>
|
||||
@ -1271,6 +1272,78 @@ protected:
|
||||
};
|
||||
|
||||
|
||||
class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name() const
|
||||
{
|
||||
return MavlinkStreamLocalPositionNEDCOV::get_name_static();
|
||||
}
|
||||
|
||||
static const char *get_name_static()
|
||||
{
|
||||
return "LOCAL_POSITION_NED_COV";
|
||||
}
|
||||
|
||||
uint8_t get_id()
|
||||
{
|
||||
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamLocalPositionNEDCOV(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size()
|
||||
{
|
||||
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *_est_sub;
|
||||
uint64_t _est_time;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &);
|
||||
MavlinkStreamLocalPositionNEDCOV& operator = (const MavlinkStreamLocalPositionNEDCOV &);
|
||||
|
||||
protected:
|
||||
explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
|
||||
_est_time(0)
|
||||
{}
|
||||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
struct estimator_status_s est;
|
||||
|
||||
if (_est_sub->update(&_est_time, &est)) {
|
||||
mavlink_local_position_ned_cov_t msg;
|
||||
|
||||
msg.time_boot_ms = est.timestamp / 1000;
|
||||
msg.x = est.states[0];
|
||||
msg.y = est.states[1];
|
||||
msg.z = est.states[2];
|
||||
msg.vx = est.states[3];
|
||||
msg.vy = est.states[4];
|
||||
msg.vz = est.states[5];
|
||||
msg.ax = est.states[6];
|
||||
msg.ay = est.states[7];
|
||||
msg.az = est.states[8];
|
||||
for (int i=0;i<9;i++) {
|
||||
msg.covariance[i] = est.covariances[i];
|
||||
}
|
||||
msg.covariance[9] = est.nan_flags;
|
||||
msg.covariance[10] = est.health_flags;
|
||||
msg.covariance[11] = est.timeout_flags;
|
||||
memcpy(msg.covariance, est.covariances, sizeof(est.covariances));
|
||||
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamAttPosMocap : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
@ -2478,6 +2551,7 @@ const StreamListItem *streams_list[] = {
|
||||
new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static),
|
||||
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
|
||||
|
||||
@ -155,6 +155,10 @@ private:
|
||||
control::BlockParamFloat _manual_thr_min;
|
||||
control::BlockParamFloat _manual_thr_max;
|
||||
|
||||
control::BlockDerivative _vel_x_deriv;
|
||||
control::BlockDerivative _vel_y_deriv;
|
||||
control::BlockDerivative _vel_z_deriv;
|
||||
|
||||
struct {
|
||||
param_t thr_min;
|
||||
param_t thr_max;
|
||||
@ -331,6 +335,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
||||
_global_vel_sp_pub(nullptr),
|
||||
_manual_thr_min(this, "MANTHR_MIN"),
|
||||
_manual_thr_max(this, "MANTHR_MAX"),
|
||||
_vel_x_deriv(this, "VELD"),
|
||||
_vel_y_deriv(this, "VELD"),
|
||||
_vel_z_deriv(this, "VELD"),
|
||||
_ref_alt(0.0f),
|
||||
_ref_timestamp(0),
|
||||
|
||||
@ -1039,6 +1046,9 @@ MulticopterPositionControl::task_main()
|
||||
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
|
||||
t_prev = t;
|
||||
|
||||
// set dt for control blocks
|
||||
setDt(dt);
|
||||
|
||||
if (_control_mode.flag_armed && !was_armed) {
|
||||
/* reset setpoints and integrals on arming */
|
||||
_reset_pos_sp = true;
|
||||
@ -1217,8 +1227,12 @@ MulticopterPositionControl::task_main()
|
||||
/* velocity error */
|
||||
math::Vector<3> vel_err = _vel_sp - _vel;
|
||||
|
||||
/* derivative of velocity error, not includes setpoint acceleration */
|
||||
math::Vector<3> vel_err_d = (_vel - _vel_prev) / dt;
|
||||
/* derivative of velocity error, /
|
||||
* does not includes setpoint acceleration */
|
||||
math::Vector<3> vel_err_d;
|
||||
vel_err_d(0) = _vel_x_deriv.update(-_vel(0));
|
||||
vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
|
||||
vel_err_d(2) = _vel_z_deriv.update(-_vel(2));
|
||||
|
||||
/* thrust vector in NED frame */
|
||||
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
|
||||
|
||||
@ -303,3 +303,11 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f);
|
||||
|
||||
/**
|
||||
* Low pass filter cut freq. for numerical velocity derivative
|
||||
*
|
||||
* @unit Hz
|
||||
* @min 0.0
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);
|
||||
|
||||
@ -366,10 +366,10 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
|
||||
#if 1
|
||||
|
||||
// Use this to debug busy CPU that keeps rescheduling with 0 period time
|
||||
if (interval < HRT_INTERVAL_MIN) {
|
||||
PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);
|
||||
PX4_BACKTRACE();
|
||||
}
|
||||
/*if (interval < HRT_INTERVAL_MIN) {*/
|
||||
/*PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);*/
|
||||
/*PX4_BACKTRACE();*/
|
||||
/*}*/
|
||||
|
||||
#endif
|
||||
entry->deadline = deadline;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user