dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

move hover_thrust_estimator to new module (mc_hover_thrust_estimator) 

* MC_HTE: unitialize with hover_thrust parameter
* MC_HTE: constrain hover thrust setter between 0.1 and 0.9
* MC_HTE: integrate with land detector and velocity controller
* MCHoverThrustEstimator: Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value.

* MC_HTE: use altitude agl threshold to start the estimator
local_position.z is relative to the origin of the EKF while dist_bottom
is above ground

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar 2020-03-11 21:20:54 -04:00 committed by GitHub
parent 4b9e6964f4
commit f9794e99f8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
74 changed files with 419 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/rc.mc_apps
View File

@ -61,6 +61,7 @@ mc_att_control start
#
# Start Multicopter Position Controller.
#
mc_hover_thrust_estimator start
mc_pos_control start
#

1
ROMFS/px4fmu_common/init.d/rc.vtol_apps
View File

@ -22,6 +22,7 @@ vtol_att_control start
mc_rate_control start vtol
mc_att_control start vtol
mc_pos_control start vtol
mc_hover_thrust_estimator start
fw_att_control start vtol
fw_pos_control_l1 start vtol

1
boards/aerotenna/ocpoc/default.cmake
View File

@ -51,6 +51,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/airmind/mindpx-v2/default.cmake
View File

@ -69,6 +69,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/atlflight/eagle/default.cmake
View File

@ -72,6 +72,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/atlflight/eagle/qurt.cmake
View File

@ -62,6 +62,7 @@ px4_add_board(
landing_target_estimator
local_position_estimator
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
muorb/adsp

1
boards/atlflight/excelsior/default.cmake
View File

@ -72,6 +72,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/atlflight/excelsior/qurt.cmake
View File

@ -62,6 +62,7 @@ px4_add_board(
landing_target_estimator
local_position_estimator
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
muorb/adsp

1
boards/av/x-v1/default.cmake
View File

@ -69,6 +69,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/beaglebone/blue/default.cmake
View File

@ -43,6 +43,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/bitcraze/crazyflie/default.cmake
View File

@ -27,6 +27,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/emlid/navio2/default.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/holybro/durandal-v1/default.cmake
View File

@ -74,6 +74,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/holybro/durandal-v1/stackcheck.cmake
View File

@ -75,6 +75,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/holybro/kakutef7/default.cmake
View File

@ -41,6 +41,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/intel/aerofc-v1/default.cmake
View File

@ -48,6 +48,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/intel/aerofc-v1/rtps.cmake
View File

@ -48,6 +48,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
micrortps_bridge

1
boards/modalai/fc-v1/default.cmake
View File

@ -67,6 +67,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/mro/ctrl-zero-f7/default.cmake
View File

@ -72,6 +72,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/mro/x21-777/default.cmake
View File

@ -66,6 +66,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/mro/x21/default.cmake
View File

@ -69,6 +69,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/nxp/fmuk66-v3/default.cmake
View File

@ -71,6 +71,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/nxp/fmurt1062-v1/default.cmake
View File

@ -63,6 +63,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/omnibus/f4sd/default.cmake
View File

@ -59,6 +59,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/px4/fmu-v2/default.cmake
View File

@ -81,6 +81,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/px4/fmu-v2/multicopter.cmake
View File

@ -49,6 +49,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/px4/fmu-v2/test.cmake
View File

@ -79,6 +79,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/px4/fmu-v3/default.cmake
View File

@ -78,6 +78,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v3/rtps.cmake
View File

@ -78,6 +78,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
micrortps_bridge

1
boards/px4/fmu-v4/default.cmake
View File

@ -72,6 +72,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v4/rtps.cmake
View File

@ -72,6 +72,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
micrortps_bridge

1
boards/px4/fmu-v4/stackcheck.cmake
View File

@ -72,6 +72,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v4pro/default.cmake
View File

@ -74,6 +74,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v4pro/rtps.cmake
View File

@ -74,6 +74,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
micrortps_bridge

1
boards/px4/fmu-v5/critmonitor.cmake
View File

@ -74,6 +74,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v5/default.cmake
View File

@ -77,6 +77,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v5/irqmonitor.cmake
View File

@ -74,6 +74,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v5/multicopter.cmake
View File

@ -63,6 +63,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/px4/fmu-v5/rtps.cmake
View File

@ -74,6 +74,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
micrortps_bridge

1
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -75,6 +75,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v5x/default.cmake
View File

@ -75,6 +75,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/fmu-v5x/p2_base_phy_LAN8742Ai.cmake
View File

@ -75,6 +75,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/raspberrypi/default.cmake
View File

@ -44,6 +44,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge

1
boards/px4/sitl/default.cmake
View File

@ -36,6 +36,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/px4/sitl/rtps.cmake
View File

@ -37,6 +37,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
micrortps_bridge

1
boards/px4/sitl/test.cmake
View File

@ -36,6 +36,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
boards/uvify/core/default.cmake
View File

@ -51,6 +51,7 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator

1
platforms/qurt/src/px4/common/commands_hil.c
View File

@ -53,6 +53,7 @@ const char *get_commands()
"commander start --hil\n"
"sensors start\n"
"ekf2 start\n"
"mc_hover_thrust_estimator start\n"
"mc_pos_control start\n"
"mc_att_control start\n"
"mc_rate_control start\n"

1
posix-configs/SITL/init/test/test_shutdown
View File

@ -25,6 +25,7 @@ land_detector start vtol
navigator start
ekf2 start
vtol_att_control start
mc_hover_thrust_estimator start
mc_pos_control start vtol
mc_att_control start vtol
mc_rate_control start vtol

1
posix-configs/bbblue/px4.config
View File

@ -67,6 +67,7 @@ navigator start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/eagle/200qx/px4.config
View File

@ -26,6 +26,7 @@ sensors start
commander start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/eagle/210qc/px4.config
View File

@ -26,6 +26,7 @@ sensors start
commander start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/eagle/flight/px4.config
View File

@ -14,6 +14,7 @@ sensors start
commander start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/eagle/hil/px4.config
View File

@ -16,6 +16,7 @@ rc_update start
commander start -hil
sensors start
ekf2 start
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/eagle/init/rcS
View File

@ -87,6 +87,7 @@ fi
qshell commander start
qshell land_detector start multicopter
qshell mc_hover_thrust_estimator start
qshell mc_pos_control start
qshell mc_att_control start
qshell mc_rate_control start

1
posix-configs/excelsior/px4.config
View File

@ -24,6 +24,7 @@ sensors start
commander start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/ocpoc/px4.config
View File

@ -32,6 +32,7 @@ navigator start
dataman start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/rpi/px4.config
View File

@ -36,6 +36,7 @@ commander start
navigator start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/rpi/px4_hil.config
View File

@ -25,6 +25,7 @@ commander start -hil
navigator start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start
mc_rate_control start

1
posix-configs/rpi/px4_test.config
View File

@ -36,6 +36,7 @@ commander start
navigator start
ekf2 start
land_detector start multicopter
mc_hover_thrust_estimator start
mc_pos_control start
mc_att_control start

1
src/lib/CMakeLists.txt
View File

@ -47,7 +47,6 @@ add_subdirectory(conversion)
add_subdirectory(drivers)
add_subdirectory(ecl)
add_subdirectory(flight_tasks)
add_subdirectory(hover_thrust_estimator)
add_subdirectory(hysteresis)
add_subdirectory(landing_slope)
add_subdirectory(led)

86
src/lib/hover_thrust_estimator/hover_thrust_estimator.cpp
View File

@ -1,86 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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 hover_thrust_estimator.cpp
*
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
*/
#include "hover_thrust_estimator.hpp"
void HoverThrustEstimator::reset()
{
_thrust = 0.f;
_acc_z = 0.f;
_hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get());
_hover_thrust_ekf.resetAccelNoise();
}
void HoverThrustEstimator::updateParams()
{
const float ht_err_init_prev = _param_hte_ht_err_init.get();
ModuleParams::updateParams();
_hover_thrust_ekf.setProcessNoiseStdDev(_param_hte_ht_noise.get());
if (fabsf(_param_hte_ht_err_init.get() - ht_err_init_prev) > FLT_EPSILON) {
_hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get());
}
_hover_thrust_ekf.setAccelInnovGate(_param_hte_acc_gate.get());
}
void HoverThrustEstimator::update(const float dt)
{
_hover_thrust_ekf.predict(dt);
ZeroOrderHoverThrustEkf::status status{};
_hover_thrust_ekf.fuseAccZ(_acc_z, _thrust, status);
publishStatus(status);
}
void HoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status)
{
hover_thrust_estimate_s status_msg{};
status_msg.timestamp = hrt_absolute_time();
status_msg.hover_thrust = status.hover_thrust;
status_msg.hover_thrust_var = status.hover_thrust_var;
status_msg.accel_innov = status.innov;
status_msg.accel_innov_var = status.innov_var;
status_msg.accel_innov_test_ratio = status.innov_test_ratio;
status_msg.accel_noise_var = status.accel_noise_var;
_hover_thrust_ekf_pub.publish(status_msg);
}

18
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -76,6 +76,7 @@ MulticopterLandDetector::MulticopterLandDetector()
_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
_paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
@ -93,6 +94,14 @@ void MulticopterLandDetector::_update_topics()
_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
if (_params.useHoverThrustEstimate) {
hover_thrust_estimate_s hte;
if (_hover_thrust_estimate_sub.update(&hte)) {
_params.hoverThrottle = hte.hover_thrust;
}
}
}
void MulticopterLandDetector::_update_params()
@ -102,9 +111,16 @@ void MulticopterLandDetector::_update_params()
_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get()));
param_get(_paramHandle.minThrottle, &_params.minThrottle);
param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
param_get(_paramHandle.landSpeed, &_params.landSpeed);
int32_t use_hover_thrust_estimate = 0;
param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);
_params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1);
if (!_params.useHoverThrustEstimate) {
param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle);
}
}
bool MulticopterLandDetector::_get_freefall_state()

4
src/modules/land_detector/MulticopterLandDetector.h
View File

@ -48,6 +48,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include "LandDetector.h"
@ -102,6 +103,7 @@ private:
param_t hoverThrottle;
param_t minManThrottle;
param_t landSpeed;
param_t useHoverThrustEstimate;
} _paramHandle{};
struct {
@ -109,6 +111,7 @@ private:
float hoverThrottle;
float minManThrottle;
float landSpeed;
bool useHoverThrustEstimate;
} _params{};
uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
@ -118,6 +121,7 @@ private:
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
actuator_controls_s _actuator_controls {};
battery_status_s _battery_status {};

18
src/lib/hover_thrust_estimator/CMakeLists.txt → src/modules/mc_hover_thrust_estimator/CMakeLists.txt
View File

@ -31,9 +31,21 @@
#
############################################################################
px4_add_library(HoverThrustEstimator
hover_thrust_estimator.cpp
px4_add_library(zero_order_hover_thrust_ekf
zero_order_hover_thrust_ekf.cpp
zero_order_hover_thrust_ekf.hpp
)
px4_add_unit_gtest(SRC zero_order_hover_thrust_ekf_test.cpp LINKLIBS HoverThrustEstimator)
px4_add_module(
MODULE modules__mc_hover_thrust_estimator
MAIN mc_hover_thrust_estimator
SRCS
MulticopterHoverThrustEstimator.cpp
MulticopterHoverThrustEstimator.hpp
DEPENDS
mathlib
px4_work_queue
zero_order_hover_thrust_ekf
)
px4_add_unit_gtest(SRC zero_order_hover_thrust_ekf_test.cpp LINKLIBS zero_order_hover_thrust_ekf)

235
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp Normal file
View File

@ -0,0 +1,235 @@
/****************************************************************************
*
* Copyright (c) 2020 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 hover_thrust_estimator.cpp
*
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
*/
#include "MulticopterHoverThrustEstimator.hpp"
#include <mathlib/mathlib.h>
MulticopterHoverThrustEstimator::MulticopterHoverThrustEstimator() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
updateParams();
reset();
}
MulticopterHoverThrustEstimator::~MulticopterHoverThrustEstimator()
{
perf_free(_cycle_perf);
}
bool MulticopterHoverThrustEstimator::init()
{
if (!_vehicle_local_position_setpoint_sub.registerCallback()) {
PX4_ERR("vehicle_local_position_setpoint callback registration failed!");
return false;
}
return true;
}
void MulticopterHoverThrustEstimator::reset()
{
_hover_thrust_ekf.setHoverThrust(_param_mpc_thr_hover.get());
_hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get());
_hover_thrust_ekf.resetAccelNoise();
}
void MulticopterHoverThrustEstimator::updateParams()
{
const float ht_err_init_prev = _param_hte_ht_err_init.get();
ModuleParams::updateParams();
_hover_thrust_ekf.setProcessNoiseStdDev(_param_hte_ht_noise.get());
if (fabsf(_param_hte_ht_err_init.get() - ht_err_init_prev) > FLT_EPSILON) {
_hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get());
}
_hover_thrust_ekf.setAccelInnovGate(_param_hte_acc_gate.get());
}
void MulticopterHoverThrustEstimator::Run()
{
if (should_exit()) {
_vehicle_local_position_setpoint_sub.unregisterCallback();
exit_and_cleanup();
return;
}
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
updateParams();
}
perf_begin(_cycle_perf);
vehicle_land_detected_s vehicle_land_detected;
vehicle_status_s vehicle_status;
vehicle_local_position_s local_pos;
if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) {
_landed = vehicle_land_detected.landed;
}
if (_vehicle_status_sub.update(&vehicle_status)) {
_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
}
if (_vehicle_local_pos_sub.update(&local_pos)) {
// This is only necessary because the landed
// flag of the land detector does not guarantee that
// the vehicle does not touch the ground anymore
// TODO: improve the landed flag
_in_air = local_pos.dist_bottom > 1.f;
}
ZeroOrderHoverThrustEkf::status status{};
if (_armed && !_landed && _in_air) {
vehicle_local_position_setpoint_s local_pos_sp;
if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
const hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _timestamp_last) / 1e6f, 0.002f, 0.2f);
_hover_thrust_ekf.predict(dt);
if (PX4_ISFINITE(local_pos.az) && PX4_ISFINITE(local_pos_sp.thrust[2])) {
// Inform the hover thrust estimator about the measured vertical
// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status);
}
}
} else {
if (!_armed) {
reset();
}
status.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate();
status.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar();
status.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar();
status.innov = NAN;
status.innov_var = NAN;
status.innov_test_ratio = NAN;
}
publishStatus(status);
perf_end(_cycle_perf);
}
void MulticopterHoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status)
{
hover_thrust_estimate_s status_msg{};
status_msg.hover_thrust = status.hover_thrust;
status_msg.hover_thrust_var = status.hover_thrust_var;
status_msg.accel_innov = status.innov;
status_msg.accel_innov_var = status.innov_var;
status_msg.accel_innov_test_ratio = status.innov_test_ratio;
status_msg.accel_noise_var = status.accel_noise_var;
status_msg.timestamp = hrt_absolute_time();
_hover_thrust_ekf_pub.publish(status_msg);
}
int MulticopterHoverThrustEstimator::task_spawn(int argc, char *argv[])
{
MulticopterHoverThrustEstimator *instance = new MulticopterHoverThrustEstimator();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int MulticopterHoverThrustEstimator::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int MulticopterHoverThrustEstimator::print_status()
{
perf_print_counter(_cycle_perf);
return 0;
}
int MulticopterHoverThrustEstimator::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("mc_hover_thrust_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int mc_hover_thrust_estimator_main(int argc, char *argv[])
{
return MulticopterHoverThrustEstimator::main(argc, argv);
}

74
src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp → src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp
View File

@ -41,48 +41,76 @@
#pragma once
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/hover_thrust_estimate.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include "zero_order_hover_thrust_ekf.hpp"
class HoverThrustEstimator : public ModuleParams
class MulticopterHoverThrustEstimator : public ModuleBase<MulticopterHoverThrustEstimator>, public ModuleParams,
public px4::WorkItem
{
public:
HoverThrustEstimator(ModuleParams *parent) :
ModuleParams(parent)
{
ZeroOrderHoverThrustEkf::status status{};
publishStatus(status);
}
~HoverThrustEstimator() = default;
MulticopterHoverThrustEstimator();
~MulticopterHoverThrustEstimator() override;
void reset();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
void update(float dt);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
void setThrust(float thrust) { _thrust = thrust; };
void setAccel(float accel) { _acc_z = accel; };
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
float getHoverThrustEstimate() const { return _hover_thrust_ekf.getHoverThrustEstimate(); }
bool init();
protected:
void updateParams() override;
/** @see ModuleBase::print_status() */
int print_status() override;
private:
void Run() override;
void updateParams() override;
void reset();
void publishStatus(ZeroOrderHoverThrustEkf::status &status);
ZeroOrderHoverThrustEkf _hover_thrust_ekf{};
float _acc_z{};
float _thrust{};
uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_setpoint_sub{this, ORB_ID(vehicle_local_position_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_local_pos_sub{ORB_ID(vehicle_local_position)};
hrt_abstime _timestamp_last{0};
bool _armed{false};
bool _landed{false};
bool _in_air{false};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::HTE_HT_NOISE>) _param_hte_ht_noise,
(ParamFloat<px4::params::HTE_ACC_GATE>) _param_hte_acc_gate,
(ParamFloat<px4::params::HTE_HT_ERR_INIT>) _param_hte_ht_err_init
(ParamFloat<px4::params::HTE_HT_ERR_INIT>) _param_hte_ht_err_init,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover
)
uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};
};

0
src/lib/hover_thrust_estimator/hover_thrust_estimator_params.c → src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c
View File

2
src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp → src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp
View File

@ -37,8 +37,6 @@
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
*/
#include <mathlib/mathlib.h>
#include "zero_order_hover_thrust_ekf.hpp"
void ZeroOrderHoverThrustEkf::predict(const float dt)

5
src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp → src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp
View File

@ -65,6 +65,7 @@
#include <matrix/matrix/math.hpp>
#include <ecl/geo/geo.h>
#include <mathlib/mathlib.h>
class ZeroOrderHoverThrustEkf
{
@ -86,13 +87,15 @@ public:
void predict(float _dt);
void fuseAccZ(float acc_z, float thrust, status &status_return);
void setHoverThrust(float hover_thrust) { _hover_thr = hover_thrust; }
void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; }
void setMeasurementNoiseStdDev(float measurement_noise) { _acc_var = measurement_noise * measurement_noise; }
void setHoverThrustStdDev(float hover_thrust_noise) { _state_var = hover_thrust_noise * hover_thrust_noise; }
void setAccelInnovGate(float gate_size) { _gate_size = gate_size; }
float getHoverThrustEstimate() const { return _hover_thr; }
float getHoverThrustEstimateVar() const { return _state_var; }
float getAccelNoiseVar() const { return _acc_var; }
private:
float _hover_thr{0.5f};

0
src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp → src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp
View File

1
src/modules/mc_pos_control/CMakeLists.txt
View File

@ -50,5 +50,4 @@ px4_add_module(
WeatherVane
CollisionPrevention
Takeoff
HoverThrustEstimator
)

44
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -45,7 +45,6 @@
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/weather_vane/WeatherVane.hpp>
#include <lib/hover_thrust_estimator/hover_thrust_estimator.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
@ -67,6 +66,7 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include "PositionControl/PositionControl.hpp"
#include "Takeoff/Takeoff.hpp"
@ -122,6 +122,7 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */
@ -159,6 +160,7 @@ private:
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
// Takeoff / Land
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
@ -184,8 +186,6 @@ private:
PositionControl _control; /**< class for core PID position control */
PositionControlStates _states{}; /**< structure containing vehicle state information for position control */
HoverThrustEstimator _hover_thrust_estimator;
hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */
bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */
@ -283,7 +283,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD"),
_hover_thrust_estimator(this),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time"))
{
if (vtol) {
@ -351,7 +350,6 @@ MulticopterPositionControl::parameters_update(bool force)
_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
_control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!
_control.setHoverThrust(_param_mpc_thr_hover.get());
// Check that the design parameters are inside the absolute maximum constraints
if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) {
@ -366,12 +364,16 @@ MulticopterPositionControl::parameters_update(bool force)
mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed");
}
if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
_param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
_param_mpc_thr_max.get()));
_param_mpc_thr_hover.commit();
mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
if (!_param_mpc_use_hte.get()) {
if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
_param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
_param_mpc_thr_max.get()));
_param_mpc_thr_hover.commit();
mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
}
_control.setHoverThrust(_param_mpc_thr_hover.get());
}
_flight_tasks.handleParameterUpdate();
@ -408,6 +410,14 @@ MulticopterPositionControl::poll_subscriptions()
_states.yaw = Eulerf(Quatf(att.q)).psi();
}
}
if (_param_mpc_use_hte.get()) {
hover_thrust_estimate_s hte;
if (_hover_thrust_estimate_sub.update(&hte)) {
_control.setHoverThrust(hte.hover_thrust);
}
}
}
void
@ -594,15 +604,6 @@ MulticopterPositionControl::Run()
_control.resetIntegral();
// reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate();
_hover_thrust_estimator.reset();
} else if (_takeoff.getTakeoffState() >= TakeoffState::flight) {
// Inform the hover thrust estimator about the measured vertical acceleration (positive acceleration is up)
if (PX4_ISFINITE(_local_pos.az)) {
_hover_thrust_estimator.setAccel(-_local_pos.az);
}
_hover_thrust_estimator.update(_dt);
}
if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
@ -649,9 +650,6 @@ MulticopterPositionControl::Run()
_flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz),
Vector3f(local_pos_sp.thrust));
// Inform the hover thrust estimator about the current thrust (positive thrust is up)
_hover_thrust_estimator.setThrust(-local_pos_sp.thrust[2]);
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint.timestamp = time_stamp_now;
_control.getAttitudeSetpoint(attitude_setpoint);

12
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -73,6 +73,18 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
*/
PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f);
/**
* Hover thrust source selector
*
* Set false to use the fixed parameter MPC_THR_HOVER
* (EXPERIMENTAL) Set true to use the value computed by the hover thrust estimator
*
* @boolean
* @category Developer
* @group Multicopter Position Control
*/
PARAM_DEFINE_INT32(MPC_USE_HTE, 0);
/**
* Thrust curve in Manual Mode
*