p82feo7wg
/
software
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: p82feo7wg:master
Branches Tags
p82feo7wg:zhaochang_branch
p82feo7wg:master
p82feo7wg:luogang_branch
p82feo7wg:wangjiaqi23
..
compare: p82feo7wg:wangjiaqi23
Branches Tags
p82feo7wg:zhaochang_branch
p82feo7wg:master
p82feo7wg:luogang_branch
p82feo7wg:wangjiaqi23

No commits in common. 'master' and 'wangjiaqi23' have entirely different histories.
master ... wangjiaqi2

2788 changed files with 6 additions and 426240 deletions
Show Stats

43
src/prometheus_px4/src/modules/airship_att_control/CMakeLists.txt
Unescape View File

@ -1,43 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__airship_att_control
MAIN airship_att_control
STACK_MAX 3500
COMPILE_FLAGS
SRCS
airship_att_control_main.cpp
DEPENDS
px4_work_queue
)

99
src/prometheus_px4/src/modules/airship_att_control/airship_att_control.hpp
Unescape View File

@ -1,99 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2018 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.
*
****************************************************************************/
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_angular_velocity.h>
using namespace time_literals;
/**
* Airship attitude control app start / stop handling function
*/
extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]);
class AirshipAttitudeControl : public ModuleBase<AirshipAttitudeControl>, public ModuleParams,
public px4::WorkItem
{
public:
AirshipAttitudeControl();
virtual ~AirshipAttitudeControl();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::print_status() */
int print_status() override;
void Run() override;
bool init();
private:
/**
* Check for parameter update and handle it.
*/
void parameter_update_poll();
void publish_actuator_controls();
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
struct actuator_controls_s _actuators {}; /**< actuator controls */
perf_counter_t _loop_perf; /**< loop performance counter */
};

213
src/prometheus_px4/src/modules/airship_att_control/airship_att_control_main.cpp
Unescape View File

@ -1,213 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 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 airship_att_control_main.cpp
* Airship attitude controller.
*
* @author Anton Erasmus <anton@flycloudline.com>
*/
#include "airship_att_control.hpp"
using namespace matrix;
AirshipAttitudeControl::AirshipAttitudeControl() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_actuators_0_pub(ORB_ID(actuator_controls_0)),
_loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control"))
{
}
AirshipAttitudeControl::~AirshipAttitudeControl()
{
perf_free(_loop_perf);
}
bool
AirshipAttitudeControl::init()
{
if (!_vehicle_angular_velocity_sub.registerCallback()) {
PX4_ERR("vehicle_angular_velocity callback registration failed!");
return false;
}
return true;
}
void
AirshipAttitudeControl::parameter_update_poll()
{
// 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();
}
}
void
AirshipAttitudeControl::publish_actuator_controls()
{
// zero actuators if not armed
if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
for (uint8_t i = 0 ; i < 4 ; i++) {
_actuators.control[i] = 0.0f;
}
} else {
_actuators.control[0] = 0.0f;
_actuators.control[1] = _manual_control_sp.x;
_actuators.control[2] = _manual_control_sp.r;
_actuators.control[3] = _manual_control_sp.z;
}
// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()
_actuators.timestamp = hrt_absolute_time();
_actuators_0_pub.publish(_actuators);
}
void
AirshipAttitudeControl::Run()
{
if (should_exit()) {
_vehicle_angular_velocity_sub.unregisterCallback();
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
/* run controller on gyro changes */
vehicle_angular_velocity_s angular_velocity;
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
const Vector3f rates{angular_velocity.xyz};
_actuators.timestamp_sample = angular_velocity.timestamp_sample;
/* run the rate controller immediately after a gyro update */
publish_actuator_controls();
/* check for updates in manual control topic */
if (_manual_control_sp_sub.updated()) {
_manual_control_sp_sub.update(&_manual_control_sp);
}
/* check for updates in vehicle status topic */
if (_vehicle_status_sub.updated()) {
_vehicle_status_sub.update(&_vehicle_status);
}
parameter_update_poll();
}
perf_end(_loop_perf);
}
int AirshipAttitudeControl::task_spawn(int argc, char *argv[])
{
AirshipAttitudeControl *instance = new AirshipAttitudeControl();
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 AirshipAttitudeControl::print_status()
{
PX4_INFO("Running");
perf_print_counter(_loop_perf);
print_message(_actuators);
return 0;
}
int AirshipAttitudeControl::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int AirshipAttitudeControl::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This implements the airship attitude and rate controller. Ideally it would
take attitude setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
Currently it is feeding the `manual_control_setpoint` topic directly to the actuators.
### Implementation
To reduce control latency, the module directly polls on the gyro topic published by the IMU driver.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("airship_att_control", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int airship_att_control_main(int argc, char *argv[])
{
return AirshipAttitudeControl::main(argc, argv);
}

42
src/prometheus_px4/src/modules/airspeed_selector/CMakeLists.txt
Unescape View File

@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__airspeed_selector
MAIN airspeed_selector
SRCS
airspeed_selector_main.cpp
DEPENDS
git_ecl
ecl_airdata
AirspeedValidator
)

675
src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_main.cpp
Unescape View File

@ -1,675 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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.
*
****************************************************************************/
#include <drivers/drv_hrt.h>
#include <ecl/airdata/WindEstimator.hpp>
#include <matrix/math.hpp>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/airspeed/airspeed.h>
#include <AirspeedValidator.hpp>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/estimator_selector_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/airspeed_wind.h>
using namespace time_literals;
static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */
using matrix::Dcmf;
using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector3f;
class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
public px4::ScheduledWorkItem
{
public:
AirspeedModule();
~AirspeedModule() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
private:
void Run() override;
static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
enum airspeed_index {
DISABLED_INDEX = -1,
GROUND_MINUS_WIND_INDEX,
FIRST_SENSOR_INDEX,
SECOND_SENSOR_INDEX,
THIRD_SENSOR_INDEX
};
uORB::Publication<airspeed_validated_s> _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/
uORB::PublicationMulti<airspeed_wind_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::Subscription _position_setpoint_sub{ORB_ID(position_setpoint)};
uORB::SubscriptionMultiArray<airspeed_s, MAX_NUM_AIRSPEED_SENSORS> _airspeed_subs{ORB_ID::airspeed};
estimator_status_s _estimator_status {};
vehicle_acceleration_s _accel {};
vehicle_air_data_s _vehicle_air_data {};
vehicle_attitude_s _vehicle_attitude {};
vehicle_land_detected_s _vehicle_land_detected {};
vehicle_local_position_s _vehicle_local_position {};
vehicle_status_s _vehicle_status {};
vtol_vehicle_status_s _vtol_vehicle_status {};
position_setpoint_s _position_setpoint {};
WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */
airspeed_wind_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */
int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/
int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/
AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */
hrt_abstime _time_now_usec{0};
int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */
int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */
bool _initialized{false}; /**< module initialized*/
bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
perf_counter_t _perf_elapsed{};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
(ParamFloat<px4::params::ASPD_TAS_NOISE>) _param_west_tas_noise,
(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
(ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
(ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
(ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
(ParamInt<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
(ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**< delay to declare airspeed valid again */
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
)
void init(); /**< initialization of the airspeed validator instances */
void check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */
void update_params(); /**< update parameters */
void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */
void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */
void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */
void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */
};
AirspeedModule::AirspeedModule():
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
// initialise parameters
update_params();
_perf_elapsed = perf_alloc(PC_ELAPSED, MODULE_NAME": elapsed");
}
AirspeedModule::~AirspeedModule()
{
ScheduleClear();
perf_free(_perf_elapsed);
}
int
AirspeedModule::task_spawn(int argc, char *argv[])
{
AirspeedModule *dev = new AirspeedModule();
// check if the trampoline is called for the first time
if (!dev) {
PX4_ERR("alloc failed");
return PX4_ERROR;
}
_object.store(dev);
dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
_task_id = task_id_is_work_queue;
return PX4_OK;
}
void
AirspeedModule::init()
{
check_for_connected_airspeed_sensors();
// Set the default sensor
if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) {
// constrain the index to the number of sensors connected
_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
if (_number_of_airspeed_sensors == 0) {
mavlink_log_info(&_mavlink_log_pub,
"No airspeed sensor detected. Switch to non-airspeed mode.");
} else {
mavlink_log_info(&_mavlink_log_pub,
"Primary airspeed index bigger than number connected sensors. Take last sensor.");
}
} else {
// set index to the one provided in the parameter ASPD_PRIMARY
_valid_airspeed_index = _param_airspeed_primary_index.get();
}
_prev_airspeed_index = _valid_airspeed_index; // used to detect a sensor switching
}
void
AirspeedModule::check_for_connected_airspeed_sensors()
{
// check for new connected airspeed sensor
int detected_airspeed_sensors = 0;
if (_param_airspeed_primary_index.get() > 0) {
for (int i = 0; i < _airspeed_subs.size(); i++) {
if (!_airspeed_subs[i].advertised()) {
break;
}
detected_airspeed_sensors = i + 1;
}
} else {
// user has selected groundspeed-windspeed as primary source, or disabled airspeed
detected_airspeed_sensors = 0;
}
_number_of_airspeed_sensors = detected_airspeed_sensors;
}
void
AirspeedModule::Run()
{
_time_now_usec = hrt_absolute_time(); // hrt time of the current cycle
// do not run the airspeed selector until 2s after system boot,
// as data from airspeed sensor and estimator may not be valid yet
if (_time_now_usec < 2_s) {
return;
}
perf_begin(_perf_elapsed);
if (!_initialized) {
init(); // initialize airspeed validator instances
_initialized = true;
}
parameter_update_s update;
if (_parameter_update_sub.update(&update)) {
update_params();
}
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
// check for new connected airspeed sensors as long as we're disarmed
if (!armed) {
check_for_connected_airspeed_sensors();
}
poll_topics();
update_wind_estimator_sideslip();
update_ground_minus_wind_airspeed();
if (_number_of_airspeed_sensors > 0) {
// disable checks if not a fixed-wing or the vehicle is landing/landed, as then airspeed can fall below stall speed
// and wind estimate isn't accurate anymore. Even better would be to have a reliable "ground_contact" detection
// for fixed-wing landings.
const bool in_air_fixed_wing = !_vehicle_land_detected.landed &&
_position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
// Prepare data for airspeed_validator
struct airspeed_validator_update_data input_data = {};
input_data.timestamp = _time_now_usec;
input_data.lpos_vx = _vehicle_local_position.vx;
input_data.lpos_vy = _vehicle_local_position.vy;
input_data.lpos_vz = _vehicle_local_position.vz;
input_data.lpos_valid = _vehicle_local_position_valid;
input_data.lpos_evh = _vehicle_local_position.evh;
input_data.lpos_evv = _vehicle_local_position.evv;
input_data.att_q[0] = _vehicle_attitude.q[0];
input_data.att_q[1] = _vehicle_attitude.q[1];
input_data.att_q[2] = _vehicle_attitude.q[2];
input_data.att_q[3] = _vehicle_attitude.q[3];
input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa;
input_data.accel_z = _accel.xyz[2];
input_data.vel_test_ratio = _estimator_status.vel_test_ratio;
input_data.mag_test_ratio = _estimator_status.mag_test_ratio;
// iterate through all airspeed sensors, poll new data from them and update their validators
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
// poll raw airspeed topic of the i-th sensor
airspeed_s airspeed_raw;
if (_airspeed_subs[i].update(&airspeed_raw)) {
input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
input_data.airspeed_timestamp = airspeed_raw.timestamp;
input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
// takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed
if (_in_takeoff_situation &&
(airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() ||
_ground_minus_wind_CAS > _param_fw_airspd_stall.get())) {
_in_takeoff_situation = false;
}
// reset takeoff_situation to true when not in air and not in fixed-wing mode
if (!in_air_fixed_wing) {
_in_takeoff_situation = true;
}
input_data.in_fixed_wing_flight = (armed && in_air_fixed_wing && !_in_takeoff_situation);
// push input data into airspeed validator
_airspeed_validator[i].update_airspeed_validator(input_data);
_time_last_airspeed_update[i] = _time_now_usec;
} else if (_time_now_usec - _time_last_airspeed_update[i] > 1_s) {
// declare airspeed invalid if more then 1s since last raw airspeed update
_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);
}
}
}
select_airspeed_and_publish();
perf_end(_perf_elapsed);
if (should_exit()) {
exit_and_cleanup();
}
}
void AirspeedModule::update_params()
{
updateParams();
_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
_wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get());
_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
// only apply manual entered airspeed scale to first airspeed measurement
// TODO: enable multiple airspeed sensors
_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
_airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
_airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
_airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get());
}
// if the airspeed scale estimation is enabled and the airspeed is valid,
// then set the scale inside the wind estimator to -1 such that it starts to estimate it
if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
if (_valid_airspeed_index > 0) {
// set it to a negative value to start estimation inside wind estimator
_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
} else {
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
_param_west_scale_estimation_on.commit_no_notification();
}
} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
if (_valid_airspeed_index > 0) {
_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
_param_west_airspeed_scale.commit_no_notification();
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f",
(double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
} else {
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
}
}
_scale_estimation_previously_on = _param_west_scale_estimation_on.get();
}
void AirspeedModule::poll_topics()
{
// use primary estimator_status
if (_estimator_selector_status_sub.updated()) {
estimator_selector_status_s estimator_selector_status;
if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
}
}
}
_estimator_status_sub.update(&_estimator_status);
_vehicle_acceleration_sub.update(&_accel);
_vehicle_air_data_sub.update(&_vehicle_air_data);
_vehicle_attitude_sub.update(&_vehicle_attitude);
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
_vehicle_status_sub.update(&_vehicle_status);
_vtol_vehicle_status_sub.update(&_vtol_vehicle_status);
_vehicle_local_position_sub.update(&_vehicle_local_position);
_position_setpoint_sub.update(&_position_setpoint);
_vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
&& (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid;
}
void AirspeedModule::update_wind_estimator_sideslip()
{
// update wind and airspeed estimator
_wind_estimator_sideslip.update(_time_now_usec);
if (_vehicle_local_position_valid && !_vtol_vehicle_status.vtol_in_rw_mode) {
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
Quatf q(_vehicle_attitude.q);
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
}
_wind_estimate_sideslip.timestamp = _time_now_usec;
float wind[2];
_wind_estimator_sideslip.get_wind(wind);
_wind_estimate_sideslip.windspeed_north = wind[0];
_wind_estimate_sideslip.windspeed_east = wind[1];
float wind_cov[2];
_wind_estimator_sideslip.get_wind_var(wind_cov);
_wind_estimate_sideslip.variance_north = wind_cov[0];
_wind_estimate_sideslip.variance_east = wind_cov[1];
_wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov();
_wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var();
_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
_wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale();
_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
}
void AirspeedModule::update_ground_minus_wind_airspeed()
{
// calculate airspeed estimate based on groundspeed-windspeed to use as fallback
const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
_vehicle_air_data.baro_temp_celcius);
}
void AirspeedModule::select_airspeed_and_publish()
{
const bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
!_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid();
const bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
_param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get();
const bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors;
if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) {
_valid_airspeed_index = airspeed_index::DISABLED_INDEX;
// loop through all sensors and take the first valid one
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
if (_airspeed_validator[i].get_airspeed_valid()) {
_valid_airspeed_index = i + 1;
break;
}
}
}
// check if airspeed based on ground-wind speed is valid and can be published
if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
(_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
|| _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
// _vehicle_local_position_valid determines if ground-wind estimate is valid
if (_vehicle_local_position_valid &&
(_param_airspeed_fallback_gw.get() || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
_valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;
} else {
_valid_airspeed_index = airspeed_index::DISABLED_INDEX;
}
}
// print warning or info, depending of whether airspeed got declared invalid or healthy
if (_valid_airspeed_index != _prev_airspeed_index &&
(_number_of_airspeed_sensors > 0 || !_vehicle_land_detected.landed) &&
_valid_airspeed_index != _prev_airspeed_index) {
if (_prev_airspeed_index > 0) {
mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected (%i, %i)", _prev_airspeed_index,
_valid_airspeed_index);
} else if (_prev_airspeed_index == 0 && _valid_airspeed_index == -1) {
mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation invalid");
} else if (_prev_airspeed_index == -1 && _valid_airspeed_index == 0) {
mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation valid");
} else {
mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor healthy, start using again (%i, %i)", _prev_airspeed_index,
_valid_airspeed_index);
}
}
_prev_airspeed_index = _valid_airspeed_index;
_prev_number_of_airspeed_sensors = _number_of_airspeed_sensors;
airspeed_validated_s airspeed_validated = {};
airspeed_validated.timestamp = _time_now_usec;
airspeed_validated.true_ground_minus_wind_m_s = NAN;
airspeed_validated.calibrated_ground_minus_wind_m_s = NAN;
airspeed_validated.indicated_airspeed_m_s = NAN;
airspeed_validated.calibrated_airspeed_m_s = NAN;
airspeed_validated.true_airspeed_m_s = NAN;
airspeed_validated.airspeed_sensor_measurement_valid = false;
airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
switch (_valid_airspeed_index) {
case airspeed_index::DISABLED_INDEX:
break;
case airspeed_index::GROUND_MINUS_WIND_INDEX:
airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_CAS;
airspeed_validated.calibrated_airspeed_m_s = _ground_minus_wind_CAS;
airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
break;
default:
airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_IAS();
airspeed_validated.calibrated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_CAS();
airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_TAS();
airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
airspeed_validated.airspeed_sensor_measurement_valid = true;
break;
}
_airspeed_validated_pub.publish(airspeed_validated);
_wind_est_pub[0].publish(_wind_estimate_sideslip);
// publish the wind estimator states from all airspeed validators
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
airspeed_wind_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
if (i == 0) {
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_1;
} else if (i == 1) {
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_2;
} else {
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_3;
}
_wind_est_pub[i + 1].publish(wind_est);
}
}
int AirspeedModule::custom_command(int argc, char *argv[])
{
if (!is_running()) {
int ret = AirspeedModule::task_spawn(argc, argv);
if (ret) {
return ret;
}
}
return print_usage("unknown command");
}
int AirspeedModule::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module provides a single airspeed_validated topic, containing indicated (IAS),
calibrated (CAS), true airspeed (TAS) and the information if the estimation currently
is invalid and if based sensor readings or on groundspeed minus windspeed.
Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
to a valid sensor in case of failure detection. For failure detection as well as for
the estimation of a scale factor from IAS to CAS, it runs several wind estimators
and also publishes those.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[])
{
return AirspeedModule::main(argc, argv);
}

186
src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_params.c
Unescape View File

@ -1,186 +0,0 @@
/**
* Airspeed Selector: Wind estimator wind process noise
*
* Wind process noise of the internal wind estimator(s) of the airspeed selector.
*
* @min 0
* @max 1
* @unit m/s^2
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
/**
* Airspeed Selector: Wind estimator true airspeed scale process noise
*
* Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector.
*
* @min 0
* @max 0.1
* @unit Hz
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
/**
* Airspeed Selector: Wind estimator true airspeed measurement noise
*
* True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector.
*
* @min 0
* @max 4
* @unit m/s
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
/**
* Airspeed Selector: Wind estimator sideslip measurement noise
*
* Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector.
*
* @min 0
* @max 1
* @unit rad
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3);
/**
* Airspeed Selector: Gate size for true airspeed fusion
*
* Sets the number of standard deviations used by the innovation consistency test.
*
* @min 1
* @max 5
* @unit SD
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3);
/**
* Airspeed Selector: Gate size for sideslip angle fusion
*
* Sets the number of standard deviations used by the innovation consistency test.
*
* @min 1
* @max 5
* @unit SD
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1);
/**
* Automatic airspeed scale estimation on
*
* Turns the automatic airspeed scale (scale from IAS to CAS) on or off. It is recommended to fly level
* altitude while performing the estimation. Set to 1 to start estimation (best when already flying).
* Set to 0 to end scale estimation. The estimated scale is then saved using the ASPD_SCALE parameter.
*
* @boolean
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0);
/**
* Airspeed scale (scale from IAS to CAS)
*
* Scale can either be entered manually, or estimated in-flight by setting ASPD_SCALE_EST to 1.
*
* @min 0.5
* @max 1.5
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_SCALE, 1.0f);
/**
* Index or primary airspeed measurement source
*
* @value -1 Disabled
* @value 0 Groundspeed minus windspeed
* @value 1 First airspeed sensor
* @value 2 Second airspeed sensor
* @value 3 Third airspeed sensor
*
* @reboot_required true
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ASPD_PRIMARY, 1);
/**
* Enable checks on airspeed sensors
*
* If set to true then the data comming from the airspeed sensors is checked for validity. Only applied if ASPD_PRIMARY > 0.
*
* @boolean
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 1);
/**
* Enable fallback to sensor-less airspeed estimation
*
* If set to true and airspeed checks are enabled, it will use a sensor-less airspeed estimation based on groundspeed
* minus windspeed if no other airspeed sensor available to fall back to.
*
* @value 0 Disable fallback to sensor-less estimation
* @value 1 Enable fallback to sensor-less estimation
* @boolean
* @group Airspeed Validator
*/
PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0);
/**
* Airspeed failsafe consistency threshold
*
* This specifies the minimum airspeed test ratio required to trigger a failsafe. Larger values make the check less sensitive,
* smaller values make it more sensitive. Start with a value of 1.0 when tuning. When tas_test_ratio is > 1.0 it indicates the
* inconsistency between predicted and measured airspeed is large enough to cause the wind EKF to reject airspeed measurements.
* The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the ASPD_FS_INTEG parameter.
*
* @min 0.5
* @max 3.0
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
/**
* Airspeed failsafe consistency delay
*
* This sets the time integral of airspeed test ratio exceedance above ASPD_FS_INNOV required to trigger a failsafe.
* For example if ASPD_FS_INNOV is 1 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will
* rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values
* make it more sensitive.
*
* @unit s
* @max 30.0
* @group Airspeed Validator
*/
PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f);
/**
* Airspeed failsafe stop delay
*
* Delay before stopping use of airspeed sensor if checks indicate sensor is bad.
*
* @unit s
* @group Airspeed Validator
* @min 1
* @max 10
*/
PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);
/**
* Airspeed failsafe start delay
*
* Delay before switching back to using airspeed sensor if checks indicate sensor is good.
* Set to a negative value to disable the re-enabling in flight.
*
* @unit s
* @group Airspeed Validator
* @min -1
* @max 1000
*/
PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);

115
src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp
Unescape View File

@ -1,115 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 AngularVelocityControl.cpp
*/
#include <AngularVelocityControl.hpp>
#include <px4_platform_common/defines.h>
using namespace matrix;
void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
_gain_p = P;
_gain_i = I;
_gain_d = D;
}
void AngularVelocityControl::setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
const matrix::Vector<bool, 3> &saturation_negative)
{
_saturation_positive = saturation_positive;
_saturation_negative = saturation_negative;
}
void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_velocity_sp,
const Vector3f &angular_acceleration, const float dt, const bool landed)
{
// angular rates error
Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity;
// P + D control
_angular_accel_sp = _gain_p.emult(angular_velocity_error) - _gain_d.emult(angular_acceleration);
// I + FF control
Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp);
// compute torque setpoint
_torque_sp = _inertia * _angular_accel_sp + torque_feedforward + angular_velocity.cross(_inertia * angular_velocity);
// update integral only if we are not landed
if (!landed) {
updateIntegral(angular_velocity_error, dt);
}
}
void AngularVelocityControl::updateIntegral(Vector3f &angular_velocity_error, const float dt)
{
for (int i = 0; i < 3; i++) {
// prevent further positive control saturation
if (_saturation_positive(i)) {
angular_velocity_error(i) = math::min(angular_velocity_error(i), 0.f);
}
// prevent further negative control saturation
if (_saturation_negative(i)) {
angular_velocity_error(i) = math::max(angular_velocity_error(i), 0.f);
}
// I term factor: reduce the I gain with increasing rate error.
// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
// change (noticeable in a bounce-back effect after a flip).
// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
// and up to 200 deg error leads to <25% reduction of I.
float i_factor = angular_velocity_error(i) / math::radians(400.f);
i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
// Perform the integration using a first order method
float angular_velocity_i = _angular_velocity_int(i) + i_factor * _gain_i(i) * angular_velocity_error(i) * dt;
// do not propagate the result if out of range or invalid
if (PX4_ISFINITE(angular_velocity_i)) {
_angular_velocity_int(i) = math::constrain(angular_velocity_i, -_lim_int(i), _lim_int(i));
}
}
}
void AngularVelocityControl::reset()
{
_angular_velocity_int.zero();
_torque_sp.zero();
_angular_accel_sp.zero();
}

149
src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp
Unescape View File

@ -1,149 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 AngularVelocityControl.hpp
*
* PID 3 axis angular velocity control.
*/
#pragma once
#include <matrix/matrix/math.hpp>
#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
class AngularVelocityControl
{
public:
AngularVelocityControl() = default;
~AngularVelocityControl() = default;
/**
* Set the control gains
* @param P 3D vector of proportional gains for body x,y,z axis
* @param I 3D vector of integral gains
* @param D 3D vector of derivative gains
*/
void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
/**
* Set the mximum absolute value of the integrator for all axes
* @param integrator_limit limit value for all axes x, y, z
*/
void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
/**
* Set direct angular velocity setpoint to torque feed forward gain
* @see _gain_ff
* @param FF 3D vector of feed forward gains for body x,y,z axis
*/
void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
/**
* Set inertia matrix
* @see _inertia
* @param inertia inertia matrix
*/
void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; };
/**
* Set saturation status
* @see _saturation_positive
* @see _saturation_negative
* @param saturation_positive positive saturation
* @param saturation_negative negative saturation
*/
void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
const matrix::Vector<bool, 3> &saturation_negative);
/**
* Run one control loop cycle calculation
* @param angular_velocity estimation of the current vehicle angular velocity
* @param angular_velocity_sp desired vehicle angular velocity setpoint
* @param angular_acceleration estimation of the current vehicle angular acceleration
* @param dt elapsed time since last update
* @param landed whether the vehicle is on the ground
*/
void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_velocity_sp,
const matrix::Vector3f &angular_acceleration, const float dt, const bool landed);
/**
* Get the desired angular acceleration
* @see _angular_accel_sp
*/
const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;};
/**
* Get the torque vector to apply to the vehicle
* @see _torque_sp
*/
const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;};
/**
* Get the integral term
* @see _torque_sp
*/
const matrix::Vector3f &getIntegral() {return _angular_velocity_int;};
/**
* Set the integral term to 0 to prevent windup
* @see _angular_velocity_int
*/
void resetIntegral() { _angular_velocity_int.zero(); }
/**
* ReSet the controller state
*/
void reset();
private:
void updateIntegral(matrix::Vector3f &angular_velocity_error, const float dt);
// Gains
matrix::Vector3f _gain_p; ///< proportional gain for all axes x, y, z
matrix::Vector3f _gain_i; ///< integral gain
matrix::Vector3f _gain_d; ///< derivative gain
matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
matrix::Vector3f _gain_ff; ///< direct angular velocity to torque feed forward gain
matrix::Matrix3f _inertia{matrix::eye<float, 3>()}; ///< inertia matrix
// States
matrix::Vector3f _angular_velocity_int;
matrix::Vector<bool, 3> _saturation_positive;
matrix::Vector<bool, 3> _saturation_negative;
// Output
matrix::Vector3f _angular_accel_sp; //< Angular acceleration setpoint computed using P and D gains
matrix::Vector3f _torque_sp; //< Torque setpoint to apply to the vehicle
};

45
src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp
Unescape View File

@ -1,45 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2019 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.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <AngularVelocityControl.hpp>
using namespace matrix;
TEST(AngularVelocityControlTest, AllZeroCase)
{
AngularVelocityControl control;
control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
Vector3f torque = control.getTorqueSetpoint();
EXPECT_EQ(torque, Vector3f());
}

41
src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt
Unescape View File

@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(AngularVelocityControl
AngularVelocityControl.cpp
)
target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(AngularVelocityControl PRIVATE mathlib)
px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl)

342
src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.cpp
Unescape View File

@ -1,342 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 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.
*
****************************************************************************/
#include "AngularVelocityController.hpp"
#include <drivers/drv_hrt.h>
#include <circuit_breaker/circuit_breaker.h>
#include <mathlib/math/Limits.hpp>
#include <mathlib/math/Functions.hpp>
#include <ecl/geo/geo.h>
using namespace matrix;
using namespace time_literals;
AngularVelocityController::AngularVelocityController() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
parameters_updated();
}
AngularVelocityController::~AngularVelocityController()
{
perf_free(_loop_perf);
}
bool
AngularVelocityController::init()
{
if (!_vehicle_angular_velocity_sub.registerCallback()) {
PX4_ERR("vehicle_angular_velocity callback registration failed!");
return false;
}
return true;
}
void
AngularVelocityController::parameters_updated()
{
// Control parameters
// The controller gain K is used to convert the parallel (P + I/s + sD) form
// to the ideal (K * [1 + 1/sTi + sTd]) form
const Vector3f k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get());
_control.setGains(
k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())),
k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())),
k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get())));
_control.setIntegratorLimit(
Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get()));
_control.setFeedForwardGain(
Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get()));
// inertia matrix
const float inertia[3][3] = {
{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
};
_control.setInertiaMatrix(matrix::Matrix3f(inertia));
// Hover thrust
if (!_param_mpc_use_hte.get()) {
_hover_thrust = _param_mpc_thr_hover.get();
}
}
void
AngularVelocityController::Run()
{
if (should_exit()) {
_vehicle_angular_velocity_sub.unregisterCallback();
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
parameters_updated();
}
/* run controller on gyro changes */
vehicle_angular_velocity_s vehicle_angular_velocity;
if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
const hrt_abstime now = hrt_absolute_time();
_timestamp_sample = vehicle_angular_velocity.timestamp_sample;
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
_last_run = now;
const Vector3f angular_velocity{vehicle_angular_velocity.xyz};
/* check for updates in other topics */
_vehicle_status_sub.update(&_vehicle_status);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
_landed = vehicle_land_detected.landed;
_maybe_landed = vehicle_land_detected.maybe_landed;
}
}
// Check for updates of hover thrust
if (_param_mpc_use_hte.get()) {
hover_thrust_estimate_s hte;
if (_hover_thrust_estimate_sub.update(&hte)) {
_hover_thrust = hte.hover_thrust;
}
}
// check angular acceleration topic
vehicle_angular_acceleration_s vehicle_angular_acceleration;
if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) {
_angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz);
}
// check rates setpoint topic
vehicle_rates_setpoint_s vehicle_rates_setpoint;
if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
_angular_velocity_sp(0) = vehicle_rates_setpoint.roll;
_angular_velocity_sp(1) = vehicle_rates_setpoint.pitch;
_angular_velocity_sp(2) = vehicle_rates_setpoint.yaw;
_thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body);
// Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G
_thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust);
}
// run the controller
if (_vehicle_control_mode.flag_control_rates_enabled) {
// reset integral if disarmed
if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_control.resetIntegral();
}
// update saturation status from mixer feedback
control_allocator_status_s control_allocator_status;
if (_control_allocator_status_sub.update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
}
}
_control.setSaturationStatus(saturation_positive, saturation_negative);
}
// run rate controller
_control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed);
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};
Vector3f integral = _control.getIntegral();
rate_ctrl_status.timestamp = hrt_absolute_time();
rate_ctrl_status.rollspeed_integ = integral(0);
rate_ctrl_status.pitchspeed_integ = integral(1);
rate_ctrl_status.yawspeed_integ = integral(2);
_rate_ctrl_status_pub.publish(rate_ctrl_status);
// publish controller output
publish_angular_acceleration_setpoint();
publish_torque_setpoint();
publish_thrust_setpoint();
}
}
perf_end(_loop_perf);
}
void
AngularVelocityController::publish_angular_acceleration_setpoint()
{
Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
v_angular_accel_sp.timestamp = hrt_absolute_time();
v_angular_accel_sp.timestamp_sample = _timestamp_sample;
v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
}
void
AngularVelocityController::publish_torque_setpoint()
{
Vector3f torque_sp = _control.getTorqueSetpoint();
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = _timestamp_sample;
v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void
AngularVelocityController::publish_thrust_setpoint()
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = _timestamp_sample;
v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
int AngularVelocityController::task_spawn(int argc, char *argv[])
{
AngularVelocityController *instance = new AngularVelocityController();
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 AngularVelocityController::print_status()
{
PX4_INFO("Running");
perf_print_counter(_loop_perf);
return 0;
}
int AngularVelocityController::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int AngularVelocityController::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This implements the angular velocity controller.
It takes angular velocity setpoints and measured angular
velocity as inputs and outputs actuator setpoints.
The controller has a PID loop for angular rate error.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
/**
* Angular velocity controller app start / stop handling function
*/
extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]);
int angular_velocity_controller_main(int argc, char *argv[])
{
return AngularVelocityController::main(argc, argv);
}

178
src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.hpp
Unescape View File

@ -1,178 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 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.
*
****************************************************************************/
#pragma once
#include <AngularVelocityControl.hpp>
#include <lib/matrix/matrix/math.hpp>
#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/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
using namespace time_literals;
class AngularVelocityController : public ModuleBase<AngularVelocityController>, public ModuleParams,
public px4::WorkItem
{
public:
AngularVelocityController();
virtual ~AngularVelocityController();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::print_status() */
int print_status() override;
void Run() override;
bool init();
private:
/**
* initialize some vectors/matrices from parameters
*/
void parameters_updated();
void vehicle_status_poll();
void publish_angular_acceleration_setpoint();
void publish_torque_setpoint();
void publish_thrust_setpoint();
void publish_actuator_controls();
AngularVelocityControl _control; ///< class for control calculations
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; /**< motor limits subscription */
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; /**< vehicle angular acceleration subscription */
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */
uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< torque setpoint publication */
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};
bool _landed{true};
bool _maybe_landed{true};
float _battery_status_scale{0.0f};
perf_counter_t _loop_perf; /**< loop duration performance counter */
matrix::Vector3f _angular_velocity_sp; /**< angular velocity setpoint */
matrix::Vector3f _angular_acceleration; /**< angular acceleration (estimated) */
matrix::Vector3f _thrust_sp; /**< thrust setpoint */
float _hover_thrust{0.5f}; /**< Normalized hover thrust **/
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
hrt_abstime _task_start{hrt_absolute_time()};
hrt_abstime _last_run{0};
hrt_abstime _timestamp_sample{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::AVC_X_P>) _param_avc_x_p,
(ParamFloat<px4::params::AVC_X_I>) _param_avc_x_i,
(ParamFloat<px4::params::AVC_X_I_LIM>) _param_avc_x_i_lim,
(ParamFloat<px4::params::AVC_X_D>) _param_avc_x_d,
(ParamFloat<px4::params::AVC_X_FF>) _param_avc_x_ff,
(ParamFloat<px4::params::AVC_X_K>) _param_avc_x_k,
(ParamFloat<px4::params::AVC_Y_P>) _param_avc_y_p,
(ParamFloat<px4::params::AVC_Y_I>) _param_avc_y_i,
(ParamFloat<px4::params::AVC_Y_I_LIM>) _param_avc_y_i_lim,
(ParamFloat<px4::params::AVC_Y_D>) _param_avc_y_d,
(ParamFloat<px4::params::AVC_Y_FF>) _param_avc_y_ff,
(ParamFloat<px4::params::AVC_Y_K>) _param_avc_y_k,
(ParamFloat<px4::params::AVC_Z_P>) _param_avc_z_p,
(ParamFloat<px4::params::AVC_Z_I>) _param_avc_z_i,
(ParamFloat<px4::params::AVC_Z_I_LIM>) _param_avc_z_i_lim,
(ParamFloat<px4::params::AVC_Z_D>) _param_avc_z_d,
(ParamFloat<px4::params::AVC_Z_FF>) _param_avc_z_ff,
(ParamFloat<px4::params::AVC_Z_K>) _param_avc_z_k,
(ParamFloat<px4::params::VM_MASS>) _param_vm_mass,
(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,
(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte
)
};

49
src/prometheus_px4/src/modules/angular_velocity_controller/CMakeLists.txt
Unescape View File

@ -1,49 +0,0 @@
############################################################################
#
# Copyright (c) 2019 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.
#
############################################################################
add_subdirectory(AngularVelocityControl)
px4_add_module(
MODULE modules__angular_velocity_control
MAIN angular_velocity_controller
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
AngularVelocityController.cpp
AngularVelocityController.hpp
DEPENDS
circuit_breaker
mathlib
AngularVelocityControl
px4_work_queue
)

297
src/prometheus_px4/src/modules/angular_velocity_controller/angular_velocity_controller_params.c
Unescape View File

@ -1,297 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 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 angular_velocity_controller_params.c
* Parameters for angular velocity controller.
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Body X axis angular velocity P gain
*
* Body X axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit 1/s
* @min 0.0
* @max 20.0
* @decimal 3
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_P, 18.f);
/**
* Body X axis angular velocity I gain
*
* Body X axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @unit Nm/rad
* @min 0.0
* @decimal 3
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_I, 0.2f);
/**
* Body X axis angular velocity integrator limit
*
* Body X axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
*
* @unit Nm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_I_LIM, 0.3f);
/**
* Body X axis angular velocity D gain
*
* Body X axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @max 2.0
* @decimal 4
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_D, 0.36f);
/**
* Body X axis angular velocity feedforward gain
*
* Improves tracking performance.
*
* @unit Nm/(rad/s)
* @min 0.0
* @decimal 4
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_FF, 0.0f);
/**
* Body X axis angular velocity controller gain
*
* Global gain of the controller.
*
* This gain scales the P, I and D terms of the controller:
* output = AVC_X_K * (AVC_X_P * error
* + AVC_X_I * error_integral
* + AVC_X_D * error_derivative)
* Set AVC_X_P=1 to implement a PID in the ideal form.
* Set AVC_X_K=1 to implement a PID in the parallel form.
*
* @min 0.0
* @max 5.0
* @decimal 4
* @increment 0.0005
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_K, 1.0f);
/**
* Body Y axis angular velocity P gain
*
* Body Y axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit 1/s
* @min 0.0
* @max 20.0
* @decimal 3
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_P, 18.f);
/**
* Body Y axis angular velocity I gain
*
* Body Y axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @unit Nm/rad
* @min 0.0
* @decimal 3
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_I, 0.2f);
/**
* Body Y axis angular velocity integrator limit
*
* Body Y axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
*
* @unit Nm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_I_LIM, 0.3f);
/**
* Body Y axis angular velocity D gain
*
* Body Y axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @max 2.0
* @decimal 4
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_D, 0.36f);
/**
* Body Y axis angular velocity feedforward
*
* Improves tracking performance.
*
* @unit Nm/(rad/s)
* @min 0.0
* @decimal 4
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_FF, 0.0f);
/**
* Body Y axis angular velocity controller gain
*
* Global gain of the controller.
*
* This gain scales the P, I and D terms of the controller:
* output = AVC_Y_K * (AVC_Y_P * error
* + AVC_Y_I * error_integral
* + AVC_Y_D * error_derivative)
* Set AVC_Y_P=1 to implement a PID in the ideal form.
* Set AVC_Y_K=1 to implement a PID in the parallel form.
*
* @min 0.0
* @max 20.0
* @decimal 4
* @increment 0.0005
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_K, 1.0f);
/**
* Body Z axis angular velocity P gain
*
* Body Z axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit 1/s
* @min 0.0
* @max 20.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_P, 7.f);
/**
* Body Z axis angular velocity I gain
*
* Body Z axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @unit Nm/rad
* @min 0.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_I, 0.1f);
/**
* Body Z axis angular velocity integrator limit
*
* Body Z axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
*
* @unit Nm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_I_LIM, 0.30f);
/**
* Body Z axis angular velocity D gain
*
* Body Z axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @max 2.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_D, 0.0f);
/**
* Body Z axis angular velocity feedforward
*
* Improves tracking performance.
*
* @unit Nm/(rad/s)
* @min 0.0
* @decimal 4
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_FF, 0.0f);
/**
* Body Z axis angular velocity controller gain
*
* Global gain of the controller.
*
* This gain scales the P, I and D terms of the controller:
* output = AVC_Z_K * (AVC_Z_P * error
* + AVC_Z_I * error_integral
* + AVC_Z_D * error_derivative)
* Set AVC_Z_P=1 to implement a PID in the ideal form.
* Set AVC_Z_K=1 to implement a PID in the parallel form.
*
* @min 0.0
* @max 5.0
* @decimal 4
* @increment 0.0005
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f);

109
src/prometheus_px4/src/modules/angular_velocity_controller/vehicle_model_params.c
Unescape View File

@ -1,109 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 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 vehicle_model_params.c
* Parameters for vehicle model.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Mass
*
* @unit kg
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_MASS, 1.f);
/**
* Inertia matrix, XX component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 0.01f);
/**
* Inertia matrix, YY component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 0.01f);
/**
* Inertia matrix, ZZ component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 0.01f);
/**
* Inertia matrix, XY component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f);
/**
* Inertia matrix, XZ component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f);
/**
* Inertia matrix, YZ component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);

45
src/prometheus_px4/src/modules/attitude_estimator_q/CMakeLists.txt
Unescape View File

@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
#############################################################################
set(MODULE_CFLAGS)
px4_add_module(
MODULE modules__attitude_estimator_q
MAIN attitude_estimator_q
COMPILE_FLAGS
STACK_MAX 1600
SRCS
attitude_estimator_q_main.cpp
DEPENDS
git_ecl
ecl_geo_lookup
)

615
src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
Unescape View File

@ -1,615 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file attitude_estimator_q_main.cpp
*
* Attitude estimator (quaternion based)
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <float.h>
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <matrix/math.hpp>
#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 <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
using matrix::Dcmf;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector3f;
using matrix::wrap_pi;
using namespace time_literals;
class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleParams, public px4::WorkItem
{
public:
AttitudeEstimatorQ();
~AttitudeEstimatorQ() override = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
private:
void Run() override;
void update_parameters(bool force = false);
bool init_attq();
bool update(float dt);
// Update magnetic declination (in rads) immediately changing yaw rotation
void update_mag_declination(float new_declination);
const float _eo_max_std_dev = 100.0f; /**< Maximum permissible standard deviation for estimated orientation */
const float _dt_min = 0.00001f;
const float _dt_max = 0.02f;
uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vision_odom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
float _mag_decl{0.0f};
float _bias_max{0.0f};
Vector3f _gyro;
Vector3f _accel;
Vector3f _mag;
Vector3f _vision_hdg;
Vector3f _mocap_hdg;
Quatf _q;
Vector3f _rates;
Vector3f _gyro_bias;
Vector3f _vel_prev;
hrt_abstime _vel_prev_t{0};
Vector3f _pos_acc;
hrt_abstime _last_time{0};
bool _inited{false};
bool _data_good{false};
bool _ext_hdg_good{false};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,
(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,
(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,
(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,
(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,
(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,
(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,
(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
)
};
AttitudeEstimatorQ::AttitudeEstimatorQ() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_vel_prev.zero();
_pos_acc.zero();
_gyro.zero();
_accel.zero();
_mag.zero();
_vision_hdg.zero();
_mocap_hdg.zero();
_q.zero();
_rates.zero();
_gyro_bias.zero();
update_parameters(true);
}
bool
AttitudeEstimatorQ::init()
{
if (!_sensors_sub.registerCallback()) {
PX4_ERR("sensor combined callback registration failed!");
return false;
}
return true;
}
void
AttitudeEstimatorQ::Run()
{
if (should_exit()) {
_sensors_sub.unregisterCallback();
exit_and_cleanup();
return;
}
sensor_combined_s sensors;
if (_sensors_sub.update(&sensors)) {
update_parameters();
// Feed validator with recent sensor data
if (sensors.timestamp > 0) {
_gyro(0) = sensors.gyro_rad[0];
_gyro(1) = sensors.gyro_rad[1];
_gyro(2) = sensors.gyro_rad[2];
}
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
_accel(0) = sensors.accelerometer_m_s2[0];
_accel(1) = sensors.accelerometer_m_s2[1];
_accel(2) = sensors.accelerometer_m_s2[2];
if (_accel.length() < 0.01f) {
PX4_ERR("degenerate accel!");
return;
}
}
// Update magnetometer
if (_magnetometer_sub.updated()) {
vehicle_magnetometer_s magnetometer;
if (_magnetometer_sub.copy(&magnetometer)) {
_mag(0) = magnetometer.magnetometer_ga[0];
_mag(1) = magnetometer.magnetometer_ga[1];
_mag(2) = magnetometer.magnetometer_ga[2];
if (_mag.length() < 0.01f) {
PX4_ERR("degenerate mag!");
return;
}
}
}
_data_good = true;
// Update vision and motion capture heading
_ext_hdg_good = false;
if (_vision_odom_sub.updated()) {
vehicle_odometry_s vision;
if (_vision_odom_sub.copy(&vision)) {
// validation check for vision attitude data
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q);
Vector3f v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transpose() * v;
// vision external heading usage (ATT_EXT_HDG_M 1)
if (_param_att_ext_hdg_m.get() == 1) {
// Check for timeouts on data
_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
}
}
}
}
if (_mocap_odom_sub.updated()) {
vehicle_odometry_s mocap;
if (_mocap_odom_sub.copy(&mocap)) {
// validation check for mocap attitude data
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
&& (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q);
Vector3f v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transpose() * v;
// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
if (_param_att_ext_hdg_m.get() == 2) {
// Check for timeouts on data
_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
}
}
}
}
if (_gps_sub.updated()) {
vehicle_gps_position_s gps;
if (_gps_sub.copy(&gps)) {
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
/* set magnetic declination automatically */
update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon));
}
}
}
if (_local_position_sub.updated()) {
vehicle_local_position_s lpos;
if (_local_position_sub.copy(&lpos)) {
if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
&& lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
/* position data is actual */
const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
/* velocity updated */
if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
/* calculate acceleration in body frame */
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = lpos.timestamp;
_vel_prev = vel;
} else {
/* position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t = 0;
}
}
}
/* time from previous iteration */
hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
_last_time = now;
if (update(dt)) {
vehicle_attitude_s att = {};
att.timestamp_sample = sensors.timestamp;
_q.copyTo(att.q);
/* the instance count is not used here */
att.timestamp = hrt_absolute_time();
_att_pub.publish(att);
}
}
}
void
AttitudeEstimatorQ::update_parameters(bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
updateParams();
// disable mag fusion if the system does not have a mag
if (_param_sys_has_mag.get() == 0) {
_param_att_w_mag.set(0.0f);
}
// if the weight is zero (=mag disabled), make sure the estimator initializes
if (_param_att_w_mag.get() < FLT_EPSILON) {
_mag(0) = 1.f;
_mag(1) = 0.f;
_mag(2) = 0.f;
}
update_mag_declination(math::radians(_param_att_mag_decl.get()));
}
}
bool
AttitudeEstimatorQ::init_attq()
{
// Rotation matrix can be easily constructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unit vector in body frame
Vector3f k = -_accel;
k.normalize();
// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
Vector3f i = (_mag - k * (_mag * k));
i.normalize();
// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
Vector3f j = k % i;
// Fill rotation matrix
Dcmf R;
R.row(0) = i;
R.row(1) = j;
R.row(2) = k;
// Convert to quaternion
_q = R;
// Compensate for magnetic declination
Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
_q = _q * decl_rotation;
_q.normalize();
if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f) {
_inited = true;
} else {
_inited = false;
}
return _inited;
}
bool
AttitudeEstimatorQ::update(float dt)
{
if (!_inited) {
if (!_data_good) {
return false;
}
return init_attq();
}
Quatf q_last = _q;
// Angular rate of correction
Vector3f corr;
float spinRate = _gyro.length();
if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
if (_param_att_ext_hdg_m.get() == 1) {
// Vision heading correction
// Project heading to global frame and extract XY component
Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);
float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
}
if (_param_att_ext_hdg_m.get() == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
}
}
if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector3f mag_earth = _q.conjugate(_mag);
float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
float gainMult = 1.0f;
const float fifty_dps = 0.873f;
if (spinRate > fifty_dps) {
gainMult = math::min(spinRate / fifty_dps, 10.0f);
}
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
}
_q.normalize();
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector3f k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
);
// If we are not using acceleration compensation based on GPS velocity,
// fuse accel data only if its norm is close to 1 g (reduces drift).
const float accel_norm_sq = _accel.norm_squared();
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
(accel_norm_sq < upper_accel_limit * upper_accel_limit))) {
corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();
}
// Gyro bias estimation
if (spinRate < 0.175f) {
_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
}
}
_rates = _gyro + _gyro_bias;
// Feed forward gyro
corr += _rates;
// Apply correction to state
_q += _q.derivative1(corr) * dt;
// Normalize quaternion
_q.normalize();
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
// Reset quaternion to last good state
_q = q_last;
_rates.zero();
_gyro_bias.zero();
return false;
}
return true;
}
void
AttitudeEstimatorQ::update_mag_declination(float new_declination)
{
// Apply initial declination or trivial rotations without changing estimation
if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
_mag_decl = new_declination;
} else {
// Immediately rotate current estimation to avoid gyro bias growth
Quatf decl_rotation = Eulerf(0.0f, 0.0f, new_declination - _mag_decl);
_q = _q * decl_rotation;
_mag_decl = new_declination;
}
}
int
AttitudeEstimatorQ::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int
AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
{
AttitudeEstimatorQ *instance = new AttitudeEstimatorQ();
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
AttitudeEstimatorQ::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Attitude estimator q.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("AttitudeEstimatorQ", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])
{
return AttitudeEstimatorQ::main(argc, argv);
}

136
src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
Unescape View File

@ -1,136 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file attitude_estimator_q_params.c
*
* Parameters for attitude estimator (quaternion based)
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
/**
* Complimentary filter accelerometer weight
*
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
/**
* Complimentary filter magnetometer weight
*
* Set to 0 to avoid using the magnetometer.
*
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
/**
* Complimentary filter external heading weight
*
* @group Attitude Q estimator
* @min 0
* @max 1
*/
PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
/**
* Complimentary filter gyroscope bias weight
*
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
/**
* Magnetic declination, in degrees
*
* This parameter is not used in normal operation,
* as the declination is looked up based on the
* GPS coordinates of the vehicle.
*
* @group Attitude Q estimator
* @unit deg
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
/**
* Automatic GPS based declination compensation
*
* @group Attitude Q estimator
* @boolean
*/
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
/**
* External heading usage mode (from Motion capture/Vision)
*
* Set to 1 to use heading estimate from vision.
* Set to 2 to use heading from motion capture.
*
* @group Attitude Q estimator
* @value 0 None
* @value 1 Vision
* @value 2 Motion Capture
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
/**
* Acceleration compensation based on GPS velocity.
*
* @group Attitude Q estimator
* @boolean
*/
PARAM_DEFINE_INT32(ATT_ACC_COMP, 1);
/**
* Gyro bias limit
*
* @group Attitude Q estimator
* @unit rad/s
* @min 0
* @max 2
* @decimal 3
*/
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);

47
src/prometheus_px4/src/modules/battery_status/CMakeLists.txt
Unescape View File

@ -1,47 +0,0 @@
############################################################################
#
# Copyright (c) 2015-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.
#
############################################################################
px4_add_module(
MODULE modules__battery_status
MAIN battery_status
SRCS
battery_status.cpp
analog_battery.cpp
MODULE_CONFIG
module.yaml
DEPENDS
battery
conversion
drivers__device
mathlib
)

167
src/prometheus_px4/src/modules/battery_status/analog_battery.cpp
Unescape View File

@ -1,167 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-2021 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.
*
****************************************************************************/
#include <stdio.h>
#include <lib/battery/battery.h>
#include "analog_battery.h"
// Defaults to use if the parameters are not set
#if BOARD_NUMBER_BRICKS > 0
#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST)
static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
#else
static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
#endif
#else
static constexpr int DEFAULT_V_CHANNEL[1] = {0};
static constexpr int DEFAULT_I_CHANNEL[1] = {0};
#endif
AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us) :
Battery(index, parent, sample_interval_us)
{
char param_name[17];
_analog_param_handles.v_offs_cur = param_find("BAT_V_OFFS_CURR");
snprintf(param_name, sizeof(param_name), "BAT%d_V_DIV", index);
_analog_param_handles.v_div = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_A_PER_V", index);
_analog_param_handles.a_per_v = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_V_CHANNEL", index);
_analog_param_handles.v_channel = param_find(param_name);
snprintf(param_name, sizeof(param_name), "BAT%d_I_CHANNEL", index);
_analog_param_handles.i_channel = param_find(param_name);
_analog_param_handles.v_div_old = param_find("BAT_V_DIV");
_analog_param_handles.a_per_v_old = param_find("BAT_A_PER_V");
_analog_param_handles.adc_channel_old = param_find("BAT_ADC_CHANNEL");
}
void
AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
int source, int priority, float throttle_normalized)
{
float voltage_v = voltage_raw * _analog_params.v_div;
float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v;
bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
(BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected,
source, priority, throttle_normalized);
}
bool AnalogBattery::is_valid()
{
#ifdef BOARD_BRICK_VALID_LIST
bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
return valid[_index - 1];
#else
// TODO: Maybe return false instead?
return true;
#endif
}
int AnalogBattery::get_voltage_channel()
{
if (_analog_params.v_channel >= 0) {
return _analog_params.v_channel;
} else {
return DEFAULT_V_CHANNEL[_index - 1];
}
}
int AnalogBattery::get_current_channel()
{
if (_analog_params.i_channel >= 0) {
return _analog_params.i_channel;
} else {
return DEFAULT_I_CHANNEL[_index - 1];
}
}
void
AnalogBattery::updateParams()
{
if (_index == 1) {
migrateParam<float>(_analog_param_handles.v_div_old, _analog_param_handles.v_div, &_analog_params.v_div_old,
&_analog_params.v_div, _first_parameter_update);
migrateParam<float>(_analog_param_handles.a_per_v_old, _analog_param_handles.a_per_v, &_analog_params.a_per_v_old,
&_analog_params.a_per_v, _first_parameter_update);
migrateParam<int32_t>(_analog_param_handles.adc_channel_old, _analog_param_handles.v_channel,
&_analog_params.adc_channel_old, &_analog_params.v_channel, _first_parameter_update);
} else {
param_get(_analog_param_handles.v_div, &_analog_params.v_div);
param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
param_get(_analog_param_handles.v_channel, &_analog_params.v_channel);
}
param_get(_analog_param_handles.i_channel, &_analog_params.i_channel);
param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur);
if (_analog_params.v_div < 0.0f) {
/* apply scaling according to defaults if set to default */
_analog_params.v_div = BOARD_BATTERY1_V_DIV;
param_set_no_notification(_analog_param_handles.v_div, &_analog_params.v_div);
if (_index == 1) {
_analog_params.v_div_old = BOARD_BATTERY1_V_DIV;
param_set_no_notification(_analog_param_handles.v_div_old, &_analog_params.v_div_old);
}
}
if (_analog_params.a_per_v < 0.0f) {
/* apply scaling according to defaults if set to default */
_analog_params.a_per_v = BOARD_BATTERY1_A_PER_V;
param_set_no_notification(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
if (_index == 1) {
_analog_params.a_per_v_old = BOARD_BATTERY1_A_PER_V;
param_set_no_notification(_analog_param_handles.a_per_v_old, &_analog_params.a_per_v_old);
}
}
Battery::updateParams();
}

100
src/prometheus_px4/src/modules/battery_status/analog_battery.h
Unescape View File

@ -1,100 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-2021 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.
*
****************************************************************************/
#pragma once
#include <battery/battery.h>
#include <parameters/param.h>
class AnalogBattery : public Battery
{
public:
AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us);
/**
* Update current battery status message.
*
* @param voltage_raw Battery voltage read from ADC, volts
* @param current_raw Voltage of current sense resistor, volts
* @param timestamp Time at which the ADC was read (use hrt_absolute_time())
* @param source The source as defined by param BAT%d_SOURCE
* @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
* @param throttle_normalized Throttle of the vehicle, between 0 and 1
*/
void updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
int source, int priority, float throttle_normalized);
/**
* Whether the ADC channel for the voltage of this battery is valid.
* Corresponds to BOARD_BRICK_VALID_LIST
*/
bool is_valid();
/**
* Which ADC channel is used for voltage reading of this battery
*/
int get_voltage_channel();
/**
* Which ADC channel is used for current reading of this battery
*/
int get_current_channel();
protected:
struct {
param_t v_offs_cur;
param_t v_div;
param_t a_per_v;
param_t v_channel;
param_t i_channel;
param_t v_div_old;
param_t a_per_v_old;
param_t adc_channel_old;
} _analog_param_handles;
struct {
float v_offs_cur;
float v_div;
float a_per_v;
int32_t v_channel;
int32_t i_channel;
float v_div_old;
float a_per_v_old;
int32_t adc_channel_old;
} _analog_params;
virtual void updateParams() override;
};

43
src/prometheus_px4/src/modules/battery_status/analog_battery_params_common.c
Unescape View File

@ -1,43 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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.
*
****************************************************************************/
/**
* Offset in volt as seen by the ADC input of the current sensor.
*
* This offset will be subtracted before calculating the battery
* current based on the voltage.
*
* @group Battery Calibration
* @decimal 8
*/
PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);

39
src/prometheus_px4/src/modules/battery_status/analog_battery_params_deprecated.c
Unescape View File

@ -1,39 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 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.
*
****************************************************************************/
/**
* This parameter is deprecated. Please use BAT1_ADC_CHANNEL.
*
* @group Battery Calibration
*/
PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);

321
src/prometheus_px4/src/modules/battery_status/battery_status.cpp
Unescape View File

@ -1,321 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 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 sensors.cpp
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomas@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
#include <px4_platform_common/log.h>
#include <lib/mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_adc.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <lib/battery/battery.h>
#include <lib/conversion/rotation.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/adc_report.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include "analog_battery.h"
using namespace time_literals;
/**
* The channel definitions (e.g., ADC_BATTERY_VOLTAGE_CHANNEL, ADC_BATTERY_CURRENT_CHANNEL, and ADC_AIRSPEED_VOLTAGE_CHANNEL) are defined in board_config.h
*/
#ifndef BOARD_NUMBER_BRICKS
#error "battery_status module requires power bricks"
#endif
#if BOARD_NUMBER_BRICKS == 0
#error "battery_status module requires power bricks"
#endif
class BatteryStatus : public ModuleBase<BatteryStatus>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
BatteryStatus();
~BatteryStatus() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
private:
void Run() override;
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< notification of parameter updates */
uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};
static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100;
static constexpr uint32_t SAMPLE_INTERVAL_US = 1_s / SAMPLE_FREQUENCY_HZ;
AnalogBattery _battery1;
#if BOARD_NUMBER_BRICKS > 1
AnalogBattery _battery2;
#endif
AnalogBattery *_analogBatteries[BOARD_NUMBER_BRICKS] {
&_battery1,
#if BOARD_NUMBER_BRICKS > 1
&_battery2,
#endif
}; // End _analogBatteries
perf_counter_t _loop_perf; /**< loop performance counter */
/**
* Check for changes in parameters.
*/
void parameter_update_poll(bool forced = false);
/**
* Poll the ADC and update readings to suit.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void adc_poll();
};
BatteryStatus::BatteryStatus() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_battery1(1, this, SAMPLE_INTERVAL_US),
#if BOARD_NUMBER_BRICKS > 1
_battery2(2, this, SAMPLE_INTERVAL_US),
#endif
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME))
{
updateParams();
}
BatteryStatus::~BatteryStatus()
{
ScheduleClear();
}
void
BatteryStatus::parameter_update_poll(bool forced)
{
// check for parameter updates
if (_parameter_update_sub.updated() || forced) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
updateParams();
}
}
void
BatteryStatus::adc_poll()
{
/* For legacy support we publish the battery_status for the Battery that is
* associated with the Brick that is the selected source for VDD_5V_IN
* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
* Like in the FMUv4
*/
/* Per Brick readings with default unread channels at 0 */
float bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
float bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
int selected_source = -1;
adc_report_s adc_report;
if (_adc_report_sub.update(&adc_report)) {
/* Read add channels we got */
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
/* Once we have subscriptions, Do this once for the lowest (highest priority
* supply on power controller) that is valid.
*/
if (selected_source < 0 && _analogBatteries[b]->is_valid()) {
/* Indicate the lowest brick (highest priority supply on power controller)
* that is valid as the one that is the selected source for the
* VDD_5V_IN
*/
selected_source = b;
}
/* look for specific channels and process the raw voltage to measurement data */
if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) {
/* Voltage in volts */
bat_voltage_adc_readings[b] = adc_report.raw_data[i] *
adc_report.v_ref /
adc_report.resolution;
} else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) {
bat_current_adc_readings[b] = adc_report.raw_data[i] *
adc_report.v_ref /
adc_report.resolution;
}
}
}
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
actuator_controls_s ctrl{};
_actuator_ctrl_0_sub.copy(&ctrl);
_analogBatteries[b]->updateBatteryStatusADC(
hrt_absolute_time(),
bat_voltage_adc_readings[b],
bat_current_adc_readings[b],
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
b,
ctrl.control[actuator_controls_s::INDEX_THROTTLE]
);
}
}
}
void
BatteryStatus::Run()
{
if (should_exit()) {
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
/* check parameters for updates */
parameter_update_poll();
/* check battery voltage */
adc_poll();
perf_end(_loop_perf);
}
int
BatteryStatus::task_spawn(int argc, char *argv[])
{
BatteryStatus *instance = new BatteryStatus();
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;
}
bool
BatteryStatus::init()
{
return _adc_report_sub.registerCallback();
}
int BatteryStatus::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int BatteryStatus::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
The provided functionality includes:
- Read the output from the ADC driver (via ioctl interface) and publish `battery_status`.
### Implementation
It runs in its own thread and polls on the currently selected gyro topic.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("battery_status", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int battery_status_main(int argc, char *argv[])
{
return BatteryStatus::main(argc, argv);
}

63
src/prometheus_px4/src/modules/battery_status/module.yaml
Unescape View File

@ -1,63 +0,0 @@
__max_num_config_instances: &max_num_config_instances 2
module_name: battery_status
parameters:
- group: Battery Calibration
definitions:
BAT${i}_V_DIV:
description:
short: Battery ${i} voltage divider (V divider)
long: |
This is the divider from battery ${i} voltage to ADC voltage.
If using e.g. Mauch power modules the value from the datasheet
can be applied straight here. A value of -1 means to use
the board default.
type: float
decimal: 8
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1.0, -1.0]
BAT${i}_A_PER_V:
description:
short: Battery ${i} current per volt (A/V)
long: |
The voltage seen by the ADC multiplied by this factor
will determine the battery current. A value of -1 means to use
the board default.
type: float
decimal: 8
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1.0, -1.0]
BAT${i}_V_CHANNEL:
description:
short: Battery ${i} Voltage ADC Channel
long: |
This parameter specifies the ADC channel used to monitor voltage of main power battery.
A value of -1 means to use the board default.
type: int32
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1, -1]
BAT${i}_I_CHANNEL:
description:
short: Battery ${i} Current ADC Channel
long: |
This parameter specifies the ADC channel used to monitor current of main power battery.
A value of -1 means to use the board default.
type: int32
reboot_required: true
num_instances: *max_num_config_instances
instance_start: 1
default: [-1, -1]

41
src/prometheus_px4/src/modules/camera_feedback/CMakeLists.txt
Unescape View File

@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__camera_feedback
MAIN camera_feedback
SRCS
CameraFeedback.cpp
CameraFeedback.hpp
DEPENDS
px4_work_queue
)

168
src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.cpp
Unescape View File

@ -1,168 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017-2019 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.
*
****************************************************************************/
#include "CameraFeedback.hpp"
CameraFeedback::CameraFeedback() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
}
bool
CameraFeedback::init()
{
if (!_trigger_sub.registerCallback()) {
PX4_ERR("camera_trigger callback registration failed!");
return false;
}
return true;
}
void
CameraFeedback::Run()
{
if (should_exit()) {
_trigger_sub.unregisterCallback();
exit_and_cleanup();
return;
}
camera_trigger_s trig{};
if (_trigger_sub.update(&trig)) {
// update geotagging subscriptions
vehicle_global_position_s gpos{};
_gpos_sub.copy(&gpos);
vehicle_attitude_s att{};
_att_sub.copy(&att);
if (trig.timestamp == 0 ||
gpos.timestamp == 0 ||
att.timestamp == 0) {
// reject until we have valid data
return;
}
camera_capture_s capture{};
// Fill timestamps
capture.timestamp = trig.timestamp;
capture.timestamp_utc = trig.timestamp_utc;
// Fill image sequence
capture.seq = trig.seq;
// Fill position data
capture.lat = gpos.lat;
capture.lon = gpos.lon;
capture.alt = gpos.alt;
if (gpos.terrain_alt_valid) {
capture.ground_distance = gpos.alt - gpos.terrain_alt;
} else {
capture.ground_distance = -1.0f;
}
// Fill attitude data
// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
capture.q[0] = att.q[0];
capture.q[1] = att.q[1];
capture.q[2] = att.q[2];
capture.q[3] = att.q[3];
capture.result = 1;
_capture_pub.publish(capture);
}
}
int
CameraFeedback::task_spawn(int argc, char *argv[])
{
CameraFeedback *instance = new CameraFeedback();
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
CameraFeedback::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int
CameraFeedback::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("camera_feedback", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[])
{
return CameraFeedback::main(argc, argv);
}

86
src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.hpp
Unescape View File

@ -1,86 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017-2019 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.
*
****************************************************************************/
/**
*
* Online and offline geotagging from camera feedback
*
* @author Mohammed Kabir <kabir@uasys.io>
*/
#pragma once
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <px4_platform_common/px4_config.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/camera_capture.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
class CameraFeedback : public ModuleBase<CameraFeedback>, public ModuleParams, public px4::WorkItem
{
public:
CameraFeedback();
~CameraFeedback() override = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
private:
void Run() override;
uORB::SubscriptionCallbackWorkItem _trigger_sub{this, ORB_ID(camera_trigger)};
uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
uORB::Publication<camera_capture_s> _capture_pub{ORB_ID(camera_capture)};
};

298
src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp
Unescape View File

@ -1,298 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 Intel Corporation. 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.
*
****************************************************************************/
#include "ArmAuthorization.h"
#include <string.h>
#include <unistd.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/px4_config.h>
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
using namespace time_literals;
static orb_advert_t *mavlink_log_pub;
static int command_ack_sub = -1;
static hrt_abstime auth_timeout;
static hrt_abstime auth_req_time;
static hrt_abstime _param_com_arm_auth_timout;
static enum arm_auth_methods _param_com_arm_auth_method;
static int32_t _param_com_arm_auth_id;
static enum {
ARM_AUTH_IDLE = 0,
ARM_AUTH_WAITING_AUTH,
ARM_AUTH_WAITING_AUTH_WITH_ACK,
ARM_AUTH_MISSION_APPROVED
} state = ARM_AUTH_IDLE;
static uint8_t *system_id;
static uint8_t _auth_method_arm_req_check();
static uint8_t _auth_method_two_arm_check();
static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
_auth_method_arm_req_check,
_auth_method_two_arm_check,
};
void arm_auth_param_update()
{
float timeout = 0;
param_get(param_find("COM_ARM_AUTH_TO"), &timeout);
_param_com_arm_auth_timout = timeout * 1_s;
int32_t auth_method = ARM_AUTH_METHOD_ARM_REQ;
param_get(param_find("COM_ARM_AUTH_MET"), &auth_method);
if (auth_method >= 0 && auth_method < ARM_AUTH_METHOD_LAST) {
_param_com_arm_auth_method = (arm_auth_methods)auth_method;
} else {
_param_com_arm_auth_method = ARM_AUTH_METHOD_ARM_REQ;
}
param_get(param_find("COM_ARM_AUTH_ID"), &_param_com_arm_auth_id);
}
static void arm_auth_request_msg_send()
{
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
vcmd.target_system = _param_com_arm_auth_id;
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
}
static uint8_t _auth_method_arm_req_check()
{
switch (state) {
case ARM_AUTH_IDLE:
/* no authentication in process? handle bellow */
break;
case ARM_AUTH_MISSION_APPROVED:
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
default:
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
/* handling ARM_AUTH_IDLE */
arm_auth_request_msg_send();
hrt_abstime now = hrt_absolute_time();
auth_req_time = now;
auth_timeout = now + _param_com_arm_auth_timout;
state = ARM_AUTH_WAITING_AUTH;
while (now < auth_timeout) {
arm_auth_update(now);
if (state != ARM_AUTH_WAITING_AUTH && state != ARM_AUTH_WAITING_AUTH_WITH_ACK) {
break;
}
/* 0.5ms */
px4_usleep(500);
now = hrt_absolute_time();
}
switch (state) {
case ARM_AUTH_WAITING_AUTH:
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
state = ARM_AUTH_IDLE;
mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
break;
default:
break;
}
return state == ARM_AUTH_MISSION_APPROVED ?
vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
static uint8_t _auth_method_two_arm_check()
{
switch (state) {
case ARM_AUTH_IDLE:
/* no authentication in process? handle bellow */
break;
case ARM_AUTH_MISSION_APPROVED:
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
case ARM_AUTH_WAITING_AUTH:
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
default:
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
/* handling ARM_AUTH_IDLE */
arm_auth_request_msg_send();
hrt_abstime now = hrt_absolute_time();
auth_req_time = now;
auth_timeout = now + _param_com_arm_auth_timout;
state = ARM_AUTH_WAITING_AUTH;
mavlink_log_info(mavlink_log_pub, "Arm auth: Requesting authorization...");
return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
}
uint8_t arm_auth_check()
{
if (_param_com_arm_auth_method < ARM_AUTH_METHOD_LAST) {
return arm_check_method[_param_com_arm_auth_method]();
}
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
void arm_auth_update(hrt_abstime now, bool param_update)
{
if (param_update) {
arm_auth_param_update();
}
switch (state) {
case ARM_AUTH_WAITING_AUTH:
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
/* handle bellow */
break;
case ARM_AUTH_MISSION_APPROVED:
if (now > auth_timeout) {
state = ARM_AUTH_IDLE;
}
return;
case ARM_AUTH_IDLE:
default:
return;
}
/*
* handling ARM_AUTH_WAITING_AUTH, ARM_AUTH_WAITING_AUTH_WITH_ACK
*/
vehicle_command_ack_s command_ack;
bool updated = false;
orb_check(command_ack_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack);
}
if (updated
&& command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
&& command_ack.target_system == *system_id
&& command_ack.timestamp > auth_req_time) {
switch (command_ack.result) {
case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS:
state = ARM_AUTH_WAITING_AUTH_WITH_ACK;
break;
case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED:
mavlink_log_info(mavlink_log_pub,
"Arm auth: Authorized for the next %" PRId32 " seconds",
command_ack.result_param2);
state = ARM_AUTH_MISSION_APPROVED;
auth_timeout = command_ack.timestamp + (command_ack.result_param2 * 1000000);
return;
case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected");
state = ARM_AUTH_IDLE;
return;
case vehicle_command_ack_s::VEHICLE_RESULT_DENIED:
default:
switch (command_ack.result_param1) {
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE:
/* Authorizer will send reason to ground station */
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied, waypoint %" PRId32 " have a invalid value",
command_ack.result_param2);
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer");
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because airspace is in use");
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather");
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC:
default:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied");
}
state = ARM_AUTH_IDLE;
return;
}
}
if (now > auth_timeout) {
mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
state = ARM_AUTH_IDLE;
}
}
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id)
{
arm_auth_param_update();
system_id = sys_id;
command_ack_sub = orb_subscribe(ORB_ID(vehicle_command_ack));
mavlink_log_pub = mav_log_pub;
}

49
src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h
Unescape View File

@ -1,49 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 Intel Corporation. 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.
*
****************************************************************************/
#pragma once
#include <stdint.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
enum arm_auth_methods {
ARM_AUTH_METHOD_ARM_REQ = 0,
ARM_AUTH_METHOD_TWO_ARM_REQ,
ARM_AUTH_METHOD_LAST
};
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *system_id);
void arm_auth_update(hrt_abstime now, bool param_update = true);
uint8_t arm_auth_check();

40
src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt
Unescape View File

@ -1,40 +0,0 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(ArmAuthorization
ArmAuthorization.cpp
)
target_include_directories(ArmAuthorization
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)

36
src/prometheus_px4/src/modules/commander/Arming/CMakeLists.txt
Unescape View File

@ -1,36 +0,0 @@
############################################################################
#
# Copyright (c) 2019 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.
#
############################################################################
add_subdirectory(PreFlightCheck)
add_subdirectory(ArmAuthorization)
add_subdirectory(HealthFlags)

37
src/prometheus_px4/src/modules/commander/Arming/HealthFlags/CMakeLists.txt
Unescape View File

@ -1,37 +0,0 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(HealthFlags
HealthFlags.cpp
)
target_include_directories(HealthFlags PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

128
src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp
Unescape View File

@ -1,128 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-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 HealthFlags.cpp
*
* Contains helper functions to efficiently set the system health flags from commander and preflight check.
*
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
*/
#include "HealthFlags.h"
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
{
PX4_DEBUG("set_health_flags: Type %llu pres=%u enabl=%u ok=%u", (long long unsigned int)subsystem_type, present,
enabled, ok);
if (present) {
status.onboard_control_sensors_present |= (uint32_t)subsystem_type;
} else {
status.onboard_control_sensors_present &= ~(uint32_t)subsystem_type;
}
if (enabled) {
status.onboard_control_sensors_enabled |= (uint32_t)subsystem_type;
} else {
status.onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type;
}
if (ok) {
status.onboard_control_sensors_health |= (uint32_t)subsystem_type;
} else {
status.onboard_control_sensors_health &= ~(uint32_t)subsystem_type;
}
}
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status)
{
set_health_flags(subsystem_type, present, status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy,
status);
}
void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status)
{
set_health_flags(subsystem_type, status.onboard_control_sensors_present & (uint32_t)subsystem_type,
status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy, status);
}
void _print_sub(const char *name, const vehicle_status_s &status, uint32_t bit)
{
PX4_INFO("%s:\t\t%s\t%s", name,
(status.onboard_control_sensors_enabled & bit) ? "EN" : " ",
(status.onboard_control_sensors_present & bit) ?
((status.onboard_control_sensors_health & bit) ? "OK" : "ERR") :
(status.onboard_control_sensors_enabled & bit) ? "OFF" : "");
}
void print_health_flags(const vehicle_status_s &status)
{
#ifndef CONSTRAINED_FLASH
PX4_INFO("DEVICE\t\tSTATUS");
PX4_INFO("----------------------------------");
_print_sub("GYRO", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
_print_sub("ACC", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC);
_print_sub("MAG", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG);
_print_sub("PRESS", status, subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE);
_print_sub("AIRSP", status, subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE);
_print_sub("GPS", status, subsystem_info_s::SUBSYSTEM_TYPE_GPS);
_print_sub("OPT", status, subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW);
_print_sub("VIO", status, subsystem_info_s::SUBSYSTEM_TYPE_CVPOSITION);
_print_sub("LASER", status, subsystem_info_s::SUBSYSTEM_TYPE_LASERPOSITION);
_print_sub("GTRUTH", status, subsystem_info_s::SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH);
_print_sub("RATES", status, subsystem_info_s::SUBSYSTEM_TYPE_ANGULARRATECONTROL);
_print_sub("ATT", status, subsystem_info_s::SUBSYSTEM_TYPE_ATTITUDESTABILIZATION);
_print_sub("YAW", status, subsystem_info_s::SUBSYSTEM_TYPE_YAWPOSITION);
_print_sub("ALTCTL", status, subsystem_info_s::SUBSYSTEM_TYPE_ALTITUDECONTROL);
_print_sub("POS", status, subsystem_info_s::SUBSYSTEM_TYPE_POSITIONCONTROL);
_print_sub("MOT", status, subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL);
_print_sub("RC ", status, subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER);
_print_sub("GYRO2", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO2);
_print_sub("ACC2", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC2);
_print_sub("MAG2", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG2);
_print_sub("GEOFENCE", status, subsystem_info_s::SUBSYSTEM_TYPE_GEOFENCE);
_print_sub("AHRS", status, subsystem_info_s::SUBSYSTEM_TYPE_AHRS);
_print_sub("TERRAIN", status, subsystem_info_s::SUBSYSTEM_TYPE_TERRAIN);
_print_sub("REVMOT", status, subsystem_info_s::SUBSYSTEM_TYPE_REVERSEMOTOR);
_print_sub("LOGGIN", status, subsystem_info_s::SUBSYSTEM_TYPE_LOGGING);
_print_sub("BATT", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY);
_print_sub("PROX", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY);
_print_sub("SATCOM", status, subsystem_info_s::SUBSYSTEM_TYPE_SATCOM);
_print_sub("PREARM", status, subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK);
_print_sub("OBSAVD", status, subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE);
#endif
}

84
src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.h
Unescape View File

@ -1,84 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 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 HealthFlags.h
*
* Contains helper functions to efficiently set the system health flags from commander and preflight check.
*
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
*/
#pragma once
#include <px4_platform_common/log.h>
#include <uORB/topics/vehicle_status.h>
struct subsystem_info_s {
// keep in sync with mavlink MAV_SYS_STATUS_SENSOR
static constexpr uint64_t SUBSYSTEM_TYPE_GYRO = 1 << 0;
static constexpr uint64_t SUBSYSTEM_TYPE_ACC = 1 << 1;
static constexpr uint64_t SUBSYSTEM_TYPE_MAG = 1 << 2;
static constexpr uint64_t SUBSYSTEM_TYPE_ABSPRESSURE = 1 << 3;
static constexpr uint64_t SUBSYSTEM_TYPE_DIFFPRESSURE = 1 << 4;
static constexpr uint64_t SUBSYSTEM_TYPE_GPS = 1 << 5;
static constexpr uint64_t SUBSYSTEM_TYPE_OPTICALFLOW = 1 << 6;
static constexpr uint64_t SUBSYSTEM_TYPE_CVPOSITION = 1 << 7;
static constexpr uint64_t SUBSYSTEM_TYPE_LASERPOSITION = 1 << 8;
static constexpr uint64_t SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 1 << 9;
static constexpr uint64_t SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1 << 10;
static constexpr uint64_t SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 1 << 11;
static constexpr uint64_t SUBSYSTEM_TYPE_YAWPOSITION = 1 << 12;
static constexpr uint64_t SUBSYSTEM_TYPE_ALTITUDECONTROL = 1 << 13;
static constexpr uint64_t SUBSYSTEM_TYPE_POSITIONCONTROL = 1 << 14;
static constexpr uint64_t SUBSYSTEM_TYPE_MOTORCONTROL = 1 << 15;
static constexpr uint64_t SUBSYSTEM_TYPE_RCRECEIVER = 1 << 16;
static constexpr uint64_t SUBSYSTEM_TYPE_GYRO2 = 1 << 17;
static constexpr uint64_t SUBSYSTEM_TYPE_ACC2 = 1 << 18;
static constexpr uint64_t SUBSYSTEM_TYPE_MAG2 = 1 << 19;
static constexpr uint64_t SUBSYSTEM_TYPE_GEOFENCE = 1 << 20;
static constexpr uint64_t SUBSYSTEM_TYPE_AHRS = 1 << 21;
static constexpr uint64_t SUBSYSTEM_TYPE_TERRAIN = 1 << 22;
static constexpr uint64_t SUBSYSTEM_TYPE_REVERSEMOTOR = 1 << 23;
static constexpr uint64_t SUBSYSTEM_TYPE_LOGGING = 1 << 24;
static constexpr uint64_t SUBSYSTEM_TYPE_SENSORBATTERY = 1 << 25;
static constexpr uint64_t SUBSYSTEM_TYPE_SENSORPROXIMITY = 1 << 26;
static constexpr uint64_t SUBSYSTEM_TYPE_SATCOM = 1 << 27;
static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28;
static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29;
};
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status);
void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status);
void print_health_flags(const vehicle_status_s &status);

54
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt
Unescape View File

@ -1,54 +0,0 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(PreFlightCheck
PreFlightCheck.cpp
checks/preArmCheck.cpp
checks/magnetometerCheck.cpp
checks/magConsistencyCheck.cpp
checks/accelerometerCheck.cpp
checks/gyroCheck.cpp
checks/baroCheck.cpp
checks/imuConsistencyCheck.cpp
checks/airspeedCheck.cpp
checks/rcCalibrationCheck.cpp
checks/powerCheck.cpp
checks/airframeCheck.cpp
checks/ekf2Check.cpp
checks/failureDetectorCheck.cpp
checks/manualControlCheck.cpp
checks/cpuResourceCheck.cpp
checks/sdcardCheck.cpp
)
target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration)

267
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
Unescape View File

@ -1,267 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-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 PreFlightCheck.cpp
*/
#include "PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
using namespace time_literals;
static constexpr unsigned max_mandatory_gyro_count = 1;
static constexpr unsigned max_optional_gyro_count = 4;
static constexpr unsigned max_mandatory_accel_count = 1;
static constexpr unsigned max_optional_accel_count = 4;
static constexpr unsigned max_mandatory_mag_count = 1;
static constexpr unsigned max_optional_mag_count = 4;
static constexpr unsigned max_mandatory_baro_count = 1;
static constexpr unsigned max_optional_baro_count = 4;
bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, bool report_failures, const bool prearm,
const hrt_abstime &time_since_boot)
{
report_failures = (report_failures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
bool failed = false;
failed = failed || !airframeCheck(mavlink_log_pub, status);
failed = failed || !sdcardCheck(mavlink_log_pub, status_flags.sd_card_detected_once, report_failures);
/* ---- MAG ---- */
{
int32_t sys_has_mag = 1;
param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
if (sys_has_mag == 1) {
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
bool report_fail = report_failures;
int32_t device_id = -1;
if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
}
report_fail = false; // only report the first failure
}
}
// TODO: highest priority mag
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
failed = true;
}
}
}
/* ---- ACCEL ---- */
{
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
const bool required = (i < max_mandatory_accel_count);
bool report_fail = report_failures;
int32_t device_id = -1;
if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
}
report_fail = false; // only report the first failure
}
}
// TODO: highest priority (from params)
}
/* ---- GYRO ---- */
{
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
const bool required = (i < max_mandatory_gyro_count);
bool report_fail = report_failures;
int32_t device_id = -1;
if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
}
report_fail = false; // only report the first failure
}
}
// TODO: highest priority (from params)
}
/* ---- BARO ---- */
{
int32_t sys_has_baro = 1;
param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
bool baro_fail_reported = false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) {
const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1);
bool report_fail = (required && report_failures && !baro_fail_reported);
int32_t device_id = -1;
if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
baro_fail_reported = true;
}
report_fail = false; // only report the first failure
}
}
}
/* ---- IMU CONSISTENCY ---- */
// To be performed after the individual sensor checks have completed
{
if (!imuConsistencyCheck(mavlink_log_pub, status, report_failures)) {
failed = true;
}
}
/* ---- AIRSPEED ---- */
/* Perform airspeed check only if circuit breaker is not engaged and it's not a rotary wing */
if (!status_flags.circuit_breaker_engaged_airspd_check &&
(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
int32_t airspeed_mode = 0;
param_get(param_find("FW_ARSP_MODE"), &airspeed_mode);
const bool optional = (airspeed_mode == 1);
int32_t max_airspeed_check_en = 0;
param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en);
float airspeed_trim = 10.0f;
param_get(param_find("FW_AIRSPD_TRIM"), &airspeed_trim);
const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en,
arming_max_airspeed_allowed)
&& !(bool)optional) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT) {
if (rcCalibrationCheck(mavlink_log_pub, report_failures, status.is_vtol) != OK) {
if (report_failures) {
mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
}
failed = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status);
status_flags.rc_calibration_valid = false;
} else {
// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
status_flags.rc_calibration_valid = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true,
!status.rc_signal_lost, status);
}
}
/* ---- SYSTEM POWER ---- */
if (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
if (!powerCheck(mavlink_log_pub, status, report_failures, prearm)) {
failed = true;
}
}
/* ---- Navigation EKF ---- */
// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
int32_t estimator_type = -1;
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) {
param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
} else {
// EKF2 is currently the only supported option for FW & VTOL
estimator_type = 2;
}
if (estimator_type == 2) {
const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) &&
ekf2CheckSensorBias(mavlink_log_pub, report_failures);
// For the first 10 seconds the ekf2 can be unhealthy, and we just mark it
// as not present.
// After that or if report_failures is true, we'll set the flags as is.
if (!ekf_healthy && time_since_boot < 10_s && !report_failures) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status);
} else {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
}
failed |= !ekf_healthy;
}
/* ---- Failure Detector ---- */
if (!failureDetectorCheck(mavlink_log_pub, status, report_failures, prearm)) {
failed = true;
}
failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
/* Report status */
return !failed;
}

124
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
Unescape View File

@ -1,124 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 PreFlightCheck.hpp
*
* Check if flight is safely possible
* if not prevent it and inform the user.
*/
#pragma once
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>
class PreFlightCheck
{
public:
PreFlightCheck() = default;
~PreFlightCheck() = default;
/**
* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
*
* The function won't fail the test if optional sensors are not found, however,
* it will fail the test if optional sensors are found but not in working condition.
*
* @param mavlink_log_pub
* Mavlink output orb handle reference for feedback when a sensor fails
* @param checkMag
* true if the magneteometer should be checked
* @param checkAcc
* true if the accelerometers should be checked
* @param checkGyro
* true if the gyroscopes should be checked
* @param checkBaro
* true if the barometer should be checked
* @param checkAirspeed
* true if the airspeed sensor should be checked
* @param checkRC
* true if the Remote Controller should be checked
* @param checkGNSS
* true if the GNSS receiver should be checked
* @param checkPower
* true if the system power should be checked
**/
static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, bool reportFailures, const bool prearm,
const hrt_abstime &time_since_boot);
struct arm_requirements_t {
bool arm_authorization = false;
bool esc_check = false;
bool global_position = false;
bool mission = false;
bool geofence = false;
};
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status,
bool report_fail = true);
private:
static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed);
static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL);
static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
const bool prearm);
static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
const bool report_fail);
static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
const bool prearm);
static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);
static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail);
};

104
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
Unescape View File

@ -1,104 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <math.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel.h>
using namespace time_literals;
bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
bool calibration_valid = false;
bool valid = true;
if (exists) {
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0);
if (!valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance);
}
}
device_id = accel.get().device_id;
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
calibration_valid = true;
} else {
calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
}
if (!calibration_valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
}
} else {
const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
+ accel.get().y * accel.get().y
+ accel.get().z * accel.get().z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
}
// this is fatal
valid = false;
}
}
} else {
if (!optional && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance);
}
}
const bool success = calibration_valid && valid;
return success;
}

58
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp
Unescape View File

@ -1,58 +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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
using namespace time_literals;
bool PreFlightCheck::airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status)
{
bool success = true;
#ifdef CONFIG_ARCH_BOARD_PX4_FMU_V2
// We no longer support VTOL on fmu-v2, so we need to warn existing users.
if (status.is_vtol) {
mavlink_log_critical(mavlink_log_pub,
"VTOL is not supported with fmu-v2, see docs.px4.io/master/en/config/firmware.html#bootloader");
success = false;
}
#endif
return success;
}

101
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp
Unescape View File

@ -1,101 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <HealthFlags.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed_validated.h>
using namespace time_literals;
bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed)
{
bool present = true;
bool success = true;
uORB::SubscriptionData<airspeed_validated_s> airspeed_validated_sub{ORB_ID(airspeed_validated)};
airspeed_validated_sub.update();
const airspeed_validated_s &airspeed_validated = airspeed_validated_sub.get();
/*
* Check if Airspeed Selector is up and running.
*/
if (hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
if (report_fail && !optional) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Selector module down.");
}
present = false;
success = false;
goto out;
}
/*
* Check if airspeed is declared valid or not by airspeed selector.
*/
if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed invalid.");
}
present = true;
success = false;
goto out;
}
/*
* Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying
* Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
* might have been removed.
*/
if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed
&& prearm) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or pitot");
}
present = true;
success = false;
goto out;
}
out:
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status);
return success;
}

73
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp
Unescape View File

@ -1,73 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <px4_defines.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_baro.h>
using namespace time_literals;
bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK);
bool valid = false;
if (exists) {
uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0);
if (!valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro %u", instance);
}
}
} else {
if (!optional && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor %u missing", instance);
}
}
if (instance == 0) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, valid, status);
}
return valid;
}

76
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp
Unescape View File

@ -1,76 +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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <lib/parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/cpuload.h>
using namespace time_literals;
bool PreFlightCheck::cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail)
{
bool success = true;
uORB::SubscriptionData<cpuload_s> cpuload_sub{ORB_ID(cpuload)};
cpuload_sub.update();
float cpuload_percent_max;
param_get(param_find("COM_CPU_MAX"), &cpuload_percent_max);
if (cpuload_percent_max > 0.f) {
if (hrt_elapsed_time(&cpuload_sub.get().timestamp) > 2_s) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Fail: No CPU load information");
}
}
const float cpuload_percent = cpuload_sub.get().load * 100.f;
if (cpuload_percent > cpuload_percent_max) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
}
}
}
return success;
}

287
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
Unescape View File

@ -1,287 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <HealthFlags.h>
#include <math.h>
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_selector_status.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_status.h>
using namespace time_literals;
bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
const bool report_fail)
{
bool success = true; // start with a pass and change to a fail if any test fails
int32_t mag_strength_check_enabled = 1;
param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
float hgt_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
float vel_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_VEL"), &vel_test_ratio_limit);
float pos_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_POS"), &pos_test_ratio_limit);
float mag_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit);
int32_t arm_without_gps = 0;
param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps);
bool gps_success = true;
bool gps_present = true;
// Get estimator status data if available and exit with a fail recorded if not
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance};
const estimator_status_s &status = status_sub.get();
if (status.timestamp == 0) {
success = false;
goto out;
}
// Check if preflight check performed by estimator has failed
if (status.pre_flt_fail_innov_heading ||
status.pre_flt_fail_innov_vel_horiz ||
status.pre_flt_fail_innov_vel_vert ||
status.pre_flt_fail_innov_height) {
if (report_fail) {
if (status.pre_flt_fail_innov_heading) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
} else if (status.pre_flt_fail_innov_vel_horiz) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");
} else if (status.pre_flt_fail_innov_vel_vert) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");
} else if (status.pre_flt_fail_innov_height) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
}
}
success = false;
goto out;
}
if ((mag_strength_check_enabled == 1) && status.pre_flt_fail_mag_field_disturbed) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
}
success = false;
goto out;
}
// check vertical position innovation test ratio
if (status.hgt_test_ratio > hgt_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
}
success = false;
goto out;
}
// check velocity innovation test ratio
if (status.vel_test_ratio > vel_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error");
}
success = false;
goto out;
}
// check horizontal position innovation test ratio
if (status.pos_test_ratio > pos_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
}
success = false;
goto out;
}
// check magnetometer innovation test ratio
if (status.mag_test_ratio > mag_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
}
success = false;
goto out;
}
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
{
const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
gps_success = ekf_gps_fusion; // default to success if gps data is fused
if (ekf_gps_check_fail) {
if (report_fail) {
// Only report the first failure to avoid spamming
const char *message = nullptr;
if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
message = "Preflight%s: GPS fix too low";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
message = "Preflight%s: not enough GPS Satellites";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
message = "Preflight%s: GPS PDOP too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
message = "Preflight%s: GPS Horizontal Pos Error too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
message = "Preflight%s: GPS Vertical Pos Error too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
message = "Preflight%s: GPS Speed Accuracy too low";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
message = "Preflight%s: GPS Horizontal Pos Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
message = "Preflight%s: GPS Vertical Pos Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
message = "Preflight%s: GPS Hor Speed Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
message = "Preflight%s: GPS Vert Speed Drift too high";
} else {
if (!ekf_gps_fusion) {
// Likely cause unknown
message = "Preflight%s: Estimator not using GPS";
gps_present = false;
} else {
// if we land here there was a new flag added and the code not updated. Show a generic message.
message = "Preflight%s: Poor GPS Quality";
}
}
if (message) {
if (!arm_without_gps) {
mavlink_log_critical(mavlink_log_pub, message, " Fail");
} else {
mavlink_log_warning(mavlink_log_pub, message, "");
}
}
}
gps_success = false;
if (!arm_without_gps) {
success = false;
goto out;
}
}
}
out:
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, !arm_without_gps, gps_success, vehicle_status);
return success;
}
bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail)
{
// Get estimator states data if available and exit with a fail recorded if not
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::SubscriptionData<estimator_sensor_bias_s> estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias), estimator_selector_status_sub.get().primary_instance};
const estimator_sensor_bias_s &bias = estimator_sensor_bias_sub.get();
if (hrt_elapsed_time(&bias.timestamp) < 30_s) {
// check accelerometer bias estimates
if (bias.accel_bias_valid) {
const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
// adjust test threshold by 3-sigma
const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f));
if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) {
if (report_fail) {
PX4_ERR("accel bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
(double)bias.accel_bias[axis_index], (double)ekf_ab_test_limit, (double)test_uncertainty);
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
}
return false;
}
}
}
// check gyro bias estimates
if (bias.gyro_bias_valid) {
const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
// adjust test threshold by 3-sigma
const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f));
if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) {
if (report_fail) {
PX4_ERR("gyro bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
(double)bias.gyro_bias[axis_index], (double)ekf_gb_test_limit, (double)test_uncertainty);
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
}
return false;
}
}
}
}
return true;
}

73
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp
Unescape View File

@ -1,73 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <systemlib/mavlink_log.h>
bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const bool report_fail, const bool prearm)
{
// Ignore failure detector check after arming
if (!prearm) {
return true;
}
if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
if (report_fail) {
if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected");
}
}
return false;
}
return true;
}

87
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp
Unescape View File

@ -1,87 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_gyro.h>
using namespace time_literals;
bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
bool calibration_valid = false;
bool valid = false;
if (exists) {
uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0);
if (!valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance);
}
}
device_id = gyro.get().device_id;
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
calibration_valid = true;
} else {
calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);
}
if (!calibration_valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u uncalibrated", instance);
}
}
} else {
if (!optional && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor %u missing", instance);
}
}
return calibration_valid && valid;
}

126
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp
Unescape View File

@ -1,126 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <HealthFlags.h>
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensors_status_imu.h>
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
const bool report_status)
{
float accel_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit);
float gyro_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit);
// Get sensor_preflight data if available and exit with a fail recorded if not
uORB::SubscriptionData<sensors_status_imu_s> sensors_status_imu_sub{ORB_ID(sensors_status_imu)};
const sensors_status_imu_s &imu = sensors_status_imu_sub.get();
// Use the difference between IMU's to detect a bad calibration.
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) {
if (imu.accel_device_ids[i] != 0) {
if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, imu.accel_healthy[i], status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, imu.accel_healthy[i], status);
}
const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];
if (accel_inconsistency_m_s_s > accel_test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u inconsistent - Check Cal", i);
if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
}
}
return false;
} else if (accel_inconsistency_m_s_s > accel_test_limit * 0.8f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accel %u inconsistent - Check Cal", i);
}
}
}
}
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) {
if (imu.gyro_device_ids[i] != 0) {
if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, imu.accel_healthy[i], status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, imu.accel_healthy[i], status);
}
const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];
if (gyro_inconsistency_rad_s > gyro_test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u inconsistent - Check Cal", i);
if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
}
}
return false;
} else if (gyro_inconsistency_rad_s > gyro_test_limit * 0.5f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyro %u inconsistent - Check Cal", i);
}
}
}
}
return true;
}

77
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp
Unescape View File

@ -1,77 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <HealthFlags.h>
#include <lib/parameters/param.h>
#include <mathlib/mathlib.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_preflight_mag.h>
// return false if the magnetomer measurements are inconsistent
bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
const bool report_status)
{
bool pass = false; // flag for result of checks
// get the sensor preflight data
uORB::SubscriptionData<sensor_preflight_mag_s> sensors_sub{ORB_ID(sensor_preflight_mag)};
sensors_sub.update();
const sensor_preflight_mag_s &sensors = sensors_sub.get();
if (sensors.timestamp == 0) {
// can happen if not advertised (yet)
pass = true;
}
// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
int32_t angle_difference_limit_deg = 90;
param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg);
pass = pass || angle_difference_limit_deg < 0; // disabled, pass check
pass = pass || sensors.mag_inconsistency_angle < math::radians<float>(angle_difference_limit_deg);
if (!pass && report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent",
static_cast<int>(math::degrees<float>(sensors.mag_inconsistency_angle)));
mavlink_log_critical(mavlink_log_pub, "Please check orientations and recalibrate");
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status);
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status);
}
return pass;
}

113
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
Unescape View File

@ -1,113 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_mag.h>
using namespace time_literals;
bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
bool calibration_valid = false;
bool valid = false;
bool is_mag_fault = false;
if (exists) {
uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0);
if (!valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass %u", instance);
}
}
device_id = magnetometer.get().device_id;
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
calibration_valid = true;
} else {
calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);
}
if (!calibration_valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance);
}
}
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (estimator_status_sub.get().mag_device_id == static_cast<uint32_t>(device_id)) {
if (estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)) {
is_mag_fault = true;
break;
}
}
}
if (is_mag_fault && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance);
}
} else {
if (!optional && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance);
}
}
const bool success = calibration_valid && valid && !is_mag_fault;
if (instance == 0) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status);
} else if (instance == 1) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status);
}
return success;
}

79
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp
Unescape View File

@ -1,79 +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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_switches.h>
using namespace time_literals;
bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail)
{
bool success = true;
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
const manual_control_switches_s &manual_control_switches = manual_control_switches_sub.get();
if (manual_control_switches.timestamp != 0) {
// check action switches
if (manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Failure: RTL switch engaged");
}
}
if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Failure: Kill switch engaged");
}
}
if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Failure: Landing gear switch set in UP position");
}
}
}
return success;
}

124
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp
Unescape View File

@ -1,124 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <lib/parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/system_power.h>
using namespace time_literals;
unsigned int countSetBits(unsigned int n)
{
unsigned int count = 0;
while (n) {
count += n & 1;
n >>= 1;
}
return count;
}
bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
const bool prearm)
{
bool success = true;
if (!prearm) {
// Ignore power check after arming.
return true;
}
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
// Ignore power check in HITL.
return true;
}
uORB::SubscriptionData<system_power_s> system_power_sub{ORB_ID(system_power)};
system_power_sub.update();
const system_power_s &system_power = system_power_sub.get();
if (system_power.timestamp != 0) {
int32_t required_power_module_count = 0;
param_get(param_find("COM_POWER_COUNT"), &required_power_module_count);
// Check avionics rail voltages (if USB isn't connected)
if (!system_power.usb_connected) {
float avionics_power_rail_voltage = system_power.voltage5v_v;
if (avionics_power_rail_voltage < 4.5f) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
(double)avionics_power_rail_voltage);
}
} else if (avionics_power_rail_voltage < 4.8f) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage);
}
} else if (avionics_power_rail_voltage > 5.4f) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage);
}
}
const int power_module_count = countSetBits(system_power.brick_valid);
if (power_module_count < required_power_module_count) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Power redundancy not met: %d instead of %" PRId32,
power_module_count, required_power_module_count);
}
}
}
} else {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "system power unavailable");
}
success = false;
}
return success;
}

190
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
Unescape View File

@ -1,190 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <ArmAuthorization.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <HealthFlags.h>
bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
{
bool prearm_ok = true;
// USB not connected
if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe"); }
prearm_ok = false;
}
// battery and system power status
if (!status_flags.circuit_breaker_engaged_power_check) {
// Fail transition if power is not good
if (!status_flags.condition_power_input_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module"); }
prearm_ok = false;
}
// main battery level
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true,
status_flags.condition_battery_healthy, status);
// Only arm if healthy
if (!status_flags.condition_battery_healthy) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); }
}
prearm_ok = false;
}
}
// Arm Requirements: mission
if (arm_requirements.mission) {
if (!status_flags.condition_auto_mission_available) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission"); }
}
prearm_ok = false;
}
if (!status_flags.condition_global_position_valid) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position"); }
}
prearm_ok = false;
}
}
if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) {
if (!status_flags.condition_global_position_valid) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required"); }
}
prearm_ok = false;
}
if (!status_flags.condition_home_position_valid) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Home position invalid"); }
}
prearm_ok = false;
}
}
// safety button
if (safety.safety_switch_available && !safety.safety_off) {
// Fail transition if we need safety switch press
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first"); }
}
prearm_ok = false;
}
if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready"); }
}
prearm_ok = false;
}
if (arm_requirements.esc_check && status_flags.condition_escs_error) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); }
prearm_ok = false;
}
}
if (arm_requirements.esc_check && status_flags.condition_escs_failure) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); }
prearm_ok = false;
}
}
if (status.is_vtol) {
if (status.in_transition_mode) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Vehicle is in transition state"); }
prearm_ok = false;
}
}
if (!status_flags.circuit_breaker_vtol_fw_arming_check
&& status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Vehicle is not in multicopter mode"); }
prearm_ok = false;
}
}
}
if (arm_requirements.geofence && status.geofence_violated) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Arming denied, vehicle outside geofence");
}
prearm_ok = false;
}
// Arm Requirements: authorization
// check last, and only if everything else has passed
if (arm_requirements.arm_authorization && prearm_ok) {
if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
// feedback provided in arm_auth_check
prearm_ok = false;
}
}
return prearm_ok;
}

235
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp
Unescape View File

@ -1,235 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/input_rc.h>
/**
* Maximum deadzone value
*/
#define RC_INPUT_MAX_DEADZONE_US 500
/**
* Minimum value
*/
#define RC_INPUT_LOWEST_MIN_US 500
/**
* Maximum value
*/
#define RC_INPUT_HIGHEST_MAX_US 2500
int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL)
{
char nbuf[20];
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
unsigned map_fail_count = 0;
const char *rc_map_mandatory[] = { /*"RC_MAP_MODE_SW",*/
/* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/
nullptr /* end marker */
};
unsigned j = 0;
/* if VTOL, check transition switch mapping */
if (isVTOL) {
param_t trans_parm = param_find("RC_MAP_TRANS_SW");
if (trans_parm == PARAM_INVALID) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC_MAP_TRANS_SW PARAMETER MISSING."); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
map_fail_count++;
} else {
int32_t transition_switch;
param_get(trans_parm, &transition_switch);
if (transition_switch < 1) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Transition switch RC_MAP_TRANS_SW not set."); }
map_fail_count++;
}
}
}
/* first check channel mappings */
while (rc_map_mandatory[j] != nullptr) {
param_t map_parm = param_find(rc_map_mandatory[j]);
if (map_parm == PARAM_INVALID) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: PARAM %s MISSING.", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
map_fail_count++;
j++;
continue;
}
int32_t mapping;
param_get(map_parm, &mapping);
if (mapping > input_rc_s::RC_INPUT_MAX_CHANNELS) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: %s >= NUMBER OF CHANNELS.", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
map_fail_count++;
}
if (mapping == 0) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: mandatory %s is unmapped.", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
map_fail_count++;
}
j++;
}
unsigned total_fail_count = 0;
unsigned channels_failed = 0;
for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
/* should the channel be enabled? */
uint8_t count = 0;
/* initialize values to values failing the check */
float param_min = 0.0f;
float param_max = 0.0f;
float param_trim = 0.0f;
float param_rev = 0.0f;
float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles_min = param_find(nbuf);
param_get(_parameter_handles_min, &param_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, &param_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, &param_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, &param_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, &param_dz);
/* assert min..center..max ordering */
if (param_min < RC_INPUT_LOWEST_MIN_US) {
count++;
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MIN < %u.", i + 1, RC_INPUT_LOWEST_MIN_US); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
}
if (param_max > RC_INPUT_HIGHEST_MAX_US) {
count++;
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MAX > %u.", i + 1, RC_INPUT_HIGHEST_MAX_US); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
}
if (param_trim < param_min) {
count++;
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM < MIN (%d/%d).", i + 1, (int)param_trim, (int)param_min); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
}
if (param_trim > param_max) {
count++;
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM > MAX (%d/%d).", i + 1, (int)param_trim, (int)param_max); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_DZ > %u.", i + 1, RC_INPUT_MAX_DEADZONE_US); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
count++;
}
total_fail_count += count;
if (count) {
channels_failed++;
}
}
if (channels_failed) {
px4_sleep(2);
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "%d config error%s for %d RC channel%s.",
total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
}
px4_usleep(100000);
}
return total_fail_count + map_fail_count;
}

74
src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp
Unescape View File

@ -1,74 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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.
*
****************************************************************************/
#include "../PreFlightCheck.hpp"
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#ifdef __PX4_DARWIN
#include <sys/param.h>
#include <sys/mount.h>
#else
#include <sys/statfs.h>
#endif
bool PreFlightCheck::sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once,
const bool report_fail)
{
bool success = true;
int32_t param_com_arm_sdcard{0};
param_get(param_find("COM_ARM_SDCARD"), &param_com_arm_sdcard);
if (param_com_arm_sdcard > 0) {
struct statfs statfs_buf;
if (!sd_card_detected_once && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
sd_card_detected_once = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
}
if (!sd_card_detected_once) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Warning! Missing FMU SD Card.");
}
if (param_com_arm_sdcard == 2) {
// disallow arming without sd card
success = false;
}
}
}
return success;
}

73
src/prometheus_px4/src/modules/commander/CMakeLists.txt
Unescape View File

@ -1,73 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
add_subdirectory(failure_detector)
add_subdirectory(Arming)
px4_add_module(
MODULE modules__commander
MAIN commander
COMPILE_FLAGS
SRCS
accelerometer_calibration.cpp
airspeed_calibration.cpp
calibration_routines.cpp
Commander.cpp
commander_helper.cpp
esc_calibration.cpp
factory_calibration_storage.cpp
gyro_calibration.cpp
level_calibration.cpp
lm_fit.cpp
mag_calibration.cpp
ManualControl.cpp
rc_calibration.cpp
state_machine_helper.cpp
worker_thread.cpp
DEPENDS
circuit_breaker
failure_detector
git_ecl
ecl_geo
hysteresis
PreFlightCheck
ArmAuthorization
HealthFlags
sensor_calibration
)
if(PX4_TESTING)
add_subdirectory(commander_tests)
endif()
px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)

4123
src/prometheus_px4/src/modules/commander/Commander.cpp
View File

File diff suppressed because it is too large Load Diff

444
src/prometheus_px4/src/modules/commander/Commander.hpp
Unescape View File

@ -1,444 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017, 2021 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.
*
****************************************************************************/
#pragma once
/* Helper classes */
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include "failure_detector/FailureDetector.hpp"
#include "ManualControl.hpp"
#include "state_machine_helper.h"
#include "worker_thread.hpp"
#include <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
// publications
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/test_motor.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/estimator_selector_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/power_button_state.h>
#include <uORB/topics/rtl_flight_time.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vtol_vehicle_status.h>
using math::constrain;
using systemlib::Hysteresis;
using namespace time_literals;
class Commander : public ModuleBase<Commander>, public ModuleParams
{
public:
Commander();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static Commander *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
void enable_hil();
void get_circuit_breaker_params();
private:
void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
transition_result_t disarm(arm_disarm_reason_t calling_reason);
transition_result_t try_mode_change(main_state_t desired_mode);
void battery_status_check();
bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us,
const bool was_valid);
void control_status_leds(bool changed, const uint8_t battery_warning);
/**
* Checks the status of all available data links and handles switching between different system telemetry states.
*/
void data_link_check();
void avoidance_check();
void esc_status_check();
void estimator_check();
bool handle_command(const vehicle_command_s &cmd);
unsigned handle_command_motor_test(const vehicle_command_s &cmd);
void mission_init();
void offboard_control_update();
void print_reject_mode(uint8_t main_state);
void reset_posvel_validity();
bool set_home_position();
bool set_home_position_alt_only();
bool set_in_air_home_position();
bool isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpos) const;
void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const;
void fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) const;
void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const;
void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const;
void setHomePosValid();
void updateHomePositionYaw(float yaw);
void update_control_mode();
void UpdateEstimateValidity();
// Set the main system state based on RC and override device inputs
transition_result_t set_main_state(bool &changed);
// Enable override (manual reversion mode) on the system
transition_result_t set_main_state_override_on(bool &changed);
// Set the system main state based on the current RC inputs
transition_result_t set_main_state_rc();
bool shutdown_if_allowed();
bool stabilization_required();
void send_parachute_command();
DEFINE_PARAMETERS(
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_t,
(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv, /*Not realy used for now*/
(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */
(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
(ParamInt<px4::params::COM_POS_FS_PROB>) _param_com_pos_fs_prob,
(ParamInt<px4::params::COM_POS_FS_GAIN>) _param_com_pos_fs_gain,
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
// Offboard
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
// Engine failure
(ParamFloat<px4::params::COM_EF_THROT>) _param_ef_throttle_thres,
(ParamFloat<px4::params::COM_EF_C2T>) _param_ef_current2throttle_thres,
(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_rc_in_off,
(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6,
// Circuit breakers
(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
(ParamInt<px4::params::CBRK_ENGINEFAIL>) _param_cbrk_enginefail,
(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
// Geofrence
(ParamInt<px4::params::GF_ACTION>) _param_geofence_action,
// Mavlink
(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist,
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr
)
enum class PrearmedMode {
DISABLED = 0,
SAFETY_BUTTON = 1,
ALWAYS = 2
};
/* Decouple update interval and hysteresis counters, all depends on intervals */
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s}; /**< wait for hotplug sensors to come online for upto 8 seconds */
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */
const int64_t POSVEL_PROBATION_MAX = 100_s; /**< maximum probation duration (usec) */
PreFlightCheck::arm_requirements_t _arm_requirements{};
hrt_abstime _valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */
hrt_abstime _last_gpos_fail_time_us{0}; /**< Last time that the global position validity recovery check failed (usec) */
hrt_abstime _last_lpos_fail_time_us{0}; /**< Last time that the local position validity recovery check failed (usec) */
hrt_abstime _last_lvel_fail_time_us{0}; /**< Last time that the local velocity validity recovery check failed (usec) */
// Probation times for position and velocity validity checks to pass if failed
hrt_abstime _gpos_probation_time_us = POSVEL_PROBATION_MIN;
hrt_abstime _lpos_probation_time_us = POSVEL_PROBATION_MIN;
hrt_abstime _lvel_probation_time_us = POSVEL_PROBATION_MIN;
/* class variables used to check for navigation failure after takeoff */
hrt_abstime _time_last_innov_pass{0}; /**< last time velocity and position innovations passed */
hrt_abstime _time_last_innov_fail{0}; /**< last time velocity and position innovations failed */
bool _nav_test_passed{false}; /**< true if the post takeoff navigation test has passed */
bool _nav_test_failed{false}; /**< true if the post takeoff navigation test has failed */
bool _geofence_loiter_on{false};
bool _geofence_rtl_on{false};
bool _geofence_land_on{false};
bool _geofence_warning_action_on{false};
bool _geofence_violated_prev{false};
FailureDetector _failure_detector;
bool _flight_termination_triggered{false};
bool _lockdown_triggered{false};
hrt_abstime _datalink_last_heartbeat_gcs{0};
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
bool _onboard_controller_lost{false};
bool _avoidance_system_lost{false};
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
int _last_esc_online_flags{-1};
int _last_esc_failure[esc_status_s::CONNECTED_ESC_MAX] {};
hrt_abstime _last_esc_status_updated{0};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
float _battery_current{0.0f};
Hysteresis _auto_disarm_landed{false};
Hysteresis _auto_disarm_killed{false};
Hysteresis _offboard_available{false};
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
bool _last_condition_local_altitude_valid{false};
bool _last_condition_local_position_valid{false};
bool _last_condition_global_position_valid{false};
bool _last_overload{false};
unsigned int _leds_counter{0};
manual_control_switches_s _manual_control_switches{};
manual_control_switches_s _last_manual_control_switches{};
ManualControl _manual_control{this};
hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost
int32_t _flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {};
hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0};
hrt_abstime _timestamp_engine_healthy{0}; ///< absolute time when engine was healty
hrt_abstime _overload_start{0}; ///< time when CPU overload started
uint32_t _counter{0};
uint8_t _heading_reset_counter{0};
bool _status_changed{true};
bool _arm_tune_played{false};
bool _was_armed{false};
bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag
bool _have_taken_off_since_arming{false};
bool _should_set_home_on_takeoff{true};
bool _system_power_usb_connected{false};
cpuload_s _cpuload{};
geofence_result_s _geofence_result{};
vehicle_land_detected_s _land_detector{};
safety_s _safety{};
vtol_vehicle_status_s _vtol_status{};
// commander publications
actuator_armed_s _armed{};
commander_state_s _internal_state{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _status{};
vehicle_status_flags_s _status_flags{};
WorkerThread _worker_thread;
// Subscriptions
uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
#if defined(BOARD_HAS_POWER_CONTROL)
uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
#endif // BOARD_HAS_POWER_CONTROL
uORB::SubscriptionData<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
uORB::SubscriptionData<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<rtl_flight_time_s> _rtl_flight_time_sub{ORB_ID(rtl_flight_time)};
// Publications
uORB::Publication<actuator_armed_s> _armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<test_motor_s> _test_motor_pub{ORB_ID(test_motor)};
uORB::Publication<vehicle_control_mode_s> _control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
orb_advert_t _mavlink_log_pub{nullptr};
};

185
src/prometheus_px4/src/modules/commander/ManualControl.cpp
Unescape View File

@ -1,185 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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.
*
****************************************************************************/
#include "ManualControl.hpp"
#include <drivers/drv_hrt.h>
using namespace time_literals;
enum OverrideBits {
OVERRIDE_AUTO_MODE_BIT = (1 << 0),
OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
};
bool ManualControl::update()
{
bool updated = false;
if (_manual_control_setpoint_sub.updated()) {
_last_manual_control_setpoint = _manual_control_setpoint;
_manual_control_setpoint_sub.copy(&_manual_control_setpoint);
updated = true;
}
_rc_available = _rc_allowed
&& _manual_control_setpoint.timestamp != 0
&& (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s));
return updated && _rc_available;
}
bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode,
const vehicle_status_s &vehicle_status)
{
const bool override_auto_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_AUTO_MODE_BIT)
&& vehicle_control_mode.flag_control_auto_enabled;
const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
&& vehicle_control_mode.flag_control_offboard_enabled;
const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) {
const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get();
const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change)
|| (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change)
|| (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change);
// Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1]
const bool throttle_moved =
(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
if (rpy_moved || (use_throttle && throttle_moved)) {
return true;
}
}
return false;
}
bool ManualControl::wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode,
const vehicle_status_s &vehicle_status,
manual_control_switches_s &manual_control_switches, const bool landed)
{
bool ret = false;
const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE;
const bool use_button = !use_stick && _param_com_arm_swisbtn.get();
const bool use_switch = !use_stick && !use_button;
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool stick_in_lower_left = use_stick
&& isThrottleLow()
&& _manual_control_setpoint.r < -.9f;
const bool arm_button_pressed = (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)
&& use_button;
const bool arm_switch_to_disarm_transition = use_switch
&& (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON)
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF);
const bool mc_manual_thrust_mode = vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& vehicle_control_mode.flag_control_manual_enabled
&& !vehicle_control_mode.flag_control_climb_rate_enabled;
if (armed
&& (landed || mc_manual_thrust_mode)
&& (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
const bool last_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
_stick_disarm_hysteresis.set_state_and_update(true, hrt_absolute_time());
const bool disarm_trigger = !last_disarm_hysteresis && _stick_disarm_hysteresis.get_state()
&& !_stick_arm_hysteresis.get_state();
if (disarm_trigger || arm_switch_to_disarm_transition) {
ret = true;
}
} else if (!arm_button_pressed) {
_stick_disarm_hysteresis.set_state_and_update(false, hrt_absolute_time());
}
return ret;
}
bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
const manual_control_switches_s &manual_control_switches, const bool landed)
{
bool ret = false;
const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE;
const bool use_button = !use_stick && _param_com_arm_swisbtn.get();
const bool use_switch = !use_stick && !use_button;
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool stick_in_lower_right = use_stick
&& isThrottleLow()
&& _manual_control_setpoint.r > .9f;
const bool arm_button_pressed = use_button
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
const bool arm_switch_to_arm_transition = use_switch
&& (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_OFF)
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
if (!armed
&& (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
const bool last_arm_hysteresis = _stick_arm_hysteresis.get_state();
_stick_arm_hysteresis.set_state_and_update(true, hrt_absolute_time());
const bool arm_trigger = !last_arm_hysteresis && _stick_arm_hysteresis.get_state()
&& !_stick_disarm_hysteresis.get_state();
if (arm_trigger || arm_switch_to_arm_transition) {
ret = true;
}
} else if (!arm_button_pressed) {
_stick_arm_hysteresis.set_state_and_update(false, hrt_absolute_time());
}
_last_manual_control_switches_arm_switch = manual_control_switches.arm_switch; // After disarm and arm check
return ret;
}
void ManualControl::updateParams()
{
ModuleParams::updateParams();
_stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
_stick_arm_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
}

101
src/prometheus_px4/src/modules/commander/ManualControl.hpp
Unescape View File

@ -1,101 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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 ManualControl.hpp
*
* @brief Logic for handling RC or Joystick input
*
* @author Matthias Grob <maetugr@gmail.com>
*/
#pragma once
#include <lib/hysteresis/hysteresis.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
class ManualControl : ModuleParams
{
public:
ManualControl(ModuleParams *parent) : ModuleParams(parent) {};
~ManualControl() override = default;
void setRCAllowed(const bool rc_allowed) { _rc_allowed = rc_allowed; }
/**
* Check for manual control input changes and process them
* @return true if there was new data
*/
bool update();
bool isRCAvailable() const { return _rc_available; }
bool isMavlink() const { return _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; }
bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status);
bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
manual_control_switches_s &manual_control_switches, const bool landed);
bool wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
const manual_control_switches_s &manual_control_switches, const bool landed);
bool isThrottleLow() const { return _last_manual_control_setpoint.z < 0.1f; }
bool isThrottleAboveCenter() const { return _last_manual_control_setpoint.z > 0.6f; }
hrt_abstime getLastRcTimestamp() const { return _last_manual_control_setpoint.timestamp; }
private:
void updateParams() override;
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
manual_control_setpoint_s _manual_control_setpoint{};
manual_control_setpoint_s _last_manual_control_setpoint{};
// Availability
bool _rc_allowed{false};
bool _rc_available{false};
// Arming/disarming
systemlib::Hysteresis _stick_disarm_hysteresis{false};
systemlib::Hysteresis _stick_arm_hysteresis{false};
uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
(ParamBool<px4::params::COM_REARM_GRACE>) _param_com_rearm_grace,
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov
)
};

605
src/prometheus_px4/src/modules/commander/accelerometer_calibration.cpp
Unescape View File

@ -1,605 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-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 accelerometer_calibration.cpp
*
* Implementation of accelerometer calibration.
*
* Transform acceleration vector to true orientation, scale and offset
*
* ===== Model =====
* accel_corr = accel_T * (accel_raw - accel_offs)
*
* accel_corr[3] - fully corrected acceleration vector in body frame
* accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
* accel_raw[3] - raw acceleration vector
* accel_offs[3] - acceleration offset vector
*
* ===== Calibration =====
*
* Reference vectors
* accel_corr_ref[6][3] = [ g 0 0 ] // nose up
* | -g 0 0 | // nose down
* | 0 g 0 | // left side down
* | 0 -g 0 | // right side down
* | 0 0 g | // on back
* [ 0 0 -g ] // level
* accel_raw_ref[6][3]
*
* accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
*
* 6 reference vectors * 3 axes = 18 equations
* 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
*
* Find accel_offs
*
* accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
*
* Find accel_T
*
* 9 unknown constants
* need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
*
* accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
*
* Solve separate system for each row of accel_T:
*
* accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
*
* A * x = b
*
* x = [ accel_T[0][i] ]
* | accel_T[1][i] |
* [ accel_T[2][i] ]
*
* b = [ accel_corr_ref[0][i] ] // One measurement per side is enough
* | accel_corr_ref[2][i] |
* [ accel_corr_ref[4][i] ]
*
* a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
*
* Matrix A is common for all three systems:
* A = [ a[0][0] a[0][1] a[0][2] ]
* | a[2][0] a[2][1] a[2][2] |
* [ a[4][0] a[4][1] a[4][2] ]
*
* x = A^-1 * b
*
* accel_T = A^-1 * g
* g = 9.80665
*
* ===== Rotation =====
*
* Calibrating using model:
* accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
*
* Actual correction:
* accel_corr = rot * accel_T * (accel_raw - accel_offs)
*
* Known: accel_T_r, accel_offs_r, rot
* Unknown: accel_T, accel_offs
*
* Solution:
* accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
* rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
* rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
* => accel_T = rot^-1 * accel_T_r * rot
* => accel_offs = rot^-1 * accel_offs_r
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "accelerometer_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include "factory_calibration_storage.h"
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <drivers/drv_hrt.h>
#include <lib/sensor_calibration/Accelerometer.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/ecl/geo/geo.h>
#include <matrix/math.hpp>
#include <lib/conversion/rotation.h>
#include <lib/parameters/param.h>
#include <lib/systemlib/err.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/vehicle_attitude.h>
using namespace matrix;
using namespace time_literals;
static constexpr char sensor_name[] {"accel"};
static constexpr unsigned MAX_ACCEL_SENS = 4;
/// Data passed to calibration worker routine
struct accel_worker_data_s {
orb_advert_t *mavlink_log_pub{nullptr};
unsigned done_count{0};
float accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
};
// Read specified number of accelerometer samples, calculate average and dispersion.
static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
unsigned orient, unsigned samples_num)
{
Vector3f accel_sum[MAX_ACCEL_SENS] {};
unsigned counts[MAX_ACCEL_SENS] {};
unsigned errcount = 0;
// sensor thermal corrections
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
sensor_correction_s sensor_correction{};
sensor_correction_sub.copy(&sensor_correction);
uORB::SubscriptionBlocking<sensor_accel_s> accel_sub[MAX_ACCEL_SENS] {
{ORB_ID(sensor_accel), 0, 0},
{ORB_ID(sensor_accel), 0, 1},
{ORB_ID(sensor_accel), 0, 2},
{ORB_ID(sensor_accel), 0, 3},
};
/* use the first sensor to pace the readout, but do per-sensor counts */
while (counts[0] < samples_num) {
if (accel_sub[0].updatedBlocking(100000)) {
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
sensor_accel_s arp;
while (accel_sub[accel_index].update(&arp)) {
// fetch optional thermal offset corrections in sensor/board frame
Vector3f offset{0, 0, 0};
sensor_correction_sub.update(&sensor_correction);
if (sensor_correction.timestamp > 0 && arp.device_id != 0) {
for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) {
if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) {
switch (correction_index) {
case 0:
offset = Vector3f{sensor_correction.accel_offset_0};
break;
case 1:
offset = Vector3f{sensor_correction.accel_offset_1};
break;
case 2:
offset = Vector3f{sensor_correction.accel_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.accel_offset_3};
break;
}
}
}
}
accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
counts[accel_index]++;
}
}
} else {
errcount++;
continue;
}
if (errcount > samples_num / 10) {
return calibrate_return_error;
}
}
// rotate sensor measurements from sensor to body frame using board rotation matrix
const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
accel_sum[s] = board_rotation * accel_sum[s];
}
for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
const Vector3f avg{accel_sum[s] / counts[s]};
avg.copyTo(accel_avg[s][orient]);
}
return calibrate_return_ok;
}
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, void *data)
{
static constexpr unsigned samples_num = 750;
accel_worker_data_s *worker_data = (accel_worker_data_s *)(data);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);
// check accel
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
switch (orientation) {
case ORIENTATION_TAIL_DOWN: // [ g, 0, 0 ]
if (worker_data->accel_ref[accel_index][ORIENTATION_TAIL_DOWN][0] < 0.f) {
calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid X-axis, check rotation", accel_index);
return calibrate_return_error;
}
break;
case ORIENTATION_NOSE_DOWN: // [ -g, 0, 0 ]
if (worker_data->accel_ref[accel_index][ORIENTATION_NOSE_DOWN][0] > 0.f) {
calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid X-axis, check rotation", accel_index);
return calibrate_return_error;
}
break;
case ORIENTATION_LEFT: // [ 0, g, 0 ]
if (worker_data->accel_ref[accel_index][ORIENTATION_LEFT][1] < 0.f) {
calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Y-axis, check rotation", accel_index);
return calibrate_return_error;
}
break;
case ORIENTATION_RIGHT: // [ 0, -g, 0 ]
if (worker_data->accel_ref[accel_index][ORIENTATION_RIGHT][1] > 0.f) {
calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Y-axis, check rotation", accel_index);
return calibrate_return_error;
}
break;
case ORIENTATION_UPSIDE_DOWN: // [ 0, 0, g ]
if (worker_data->accel_ref[accel_index][ORIENTATION_UPSIDE_DOWN][2] < 0.f) {
calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Z-axis, check rotation", accel_index);
return calibrate_return_error;
}
break;
case ORIENTATION_RIGHTSIDE_UP: // [ 0, 0, -g ]
if (worker_data->accel_ref[accel_index][ORIENTATION_RIGHTSIDE_UP][2] > 0.f) {
calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Z-axis, check rotation", accel_index);
return calibrate_return_error;
}
break;
default:
break;
}
}
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%.3f %.3f %.3f]",
detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
return calibrate_return_ok;
}
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {};
unsigned active_sensors = 0;
for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) {
uORB::SubscriptionData<sensor_accel_s> accel_sub{ORB_ID(sensor_accel), cur_accel};
if (accel_sub.advertised() && (accel_sub.get().device_id != 0) && (accel_sub.get().timestamp > 0)) {
calibrations[cur_accel].set_device_id(accel_sub.get().device_id);
active_sensors++;
} else {
calibrations[cur_accel].Reset();
}
// reset calibration index to match uORB numbering
calibrations[cur_accel].set_calibration_index(cur_accel);
}
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return PX4_ERROR;
}
FactoryCalibrationStorage factory_storage;
if (factory_storage.open() != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
return PX4_ERROR;
}
/* measure and calculate offsets & scales */
accel_worker_data_s worker_data{};
worker_data.mavlink_log_pub = mavlink_log_pub;
bool data_collected[detect_orientation_side_count] {};
if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data,
false) == calibrate_return_ok) {
const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
const Dcmf board_rotation_t = board_rotation.transpose();
bool param_save = false;
bool failed = true;
for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) {
if (i < active_sensors) {
// calculate offsets
Vector3f offset{};
// X offset: average X from TAIL_DOWN + NOSE_DOWN
const Vector3f accel_tail_down{worker_data.accel_ref[i][ORIENTATION_TAIL_DOWN]};
const Vector3f accel_nose_down{worker_data.accel_ref[i][ORIENTATION_NOSE_DOWN]};
offset(0) = (accel_tail_down(0) + accel_nose_down(0)) * 0.5f;
// Y offset: average Y from LEFT + RIGHT
const Vector3f accel_left{worker_data.accel_ref[i][ORIENTATION_LEFT]};
const Vector3f accel_right{worker_data.accel_ref[i][ORIENTATION_RIGHT]};
offset(1) = (accel_left(1) + accel_right(1)) * 0.5f;
// Z offset: average Z from UPSIDE_DOWN + RIGHTSIDE_UP
const Vector3f accel_upside_down{worker_data.accel_ref[i][ORIENTATION_UPSIDE_DOWN]};
const Vector3f accel_rightside_up{worker_data.accel_ref[i][ORIENTATION_RIGHTSIDE_UP]};
offset(2) = (accel_upside_down(2) + accel_rightside_up(2)) * 0.5f;
// transform matrix
Matrix3f mat_A;
mat_A.row(0) = accel_tail_down - offset;
mat_A.row(1) = accel_left - offset;
mat_A.row(2) = accel_upside_down - offset;
// calculate inverse matrix for A: simplify matrices mult because b has only one non-zero element == g at index i
const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G;
// update calibration
const Vector3f accel_offs_rotated{board_rotation_t *offset};
calibrations[i].set_offset(accel_offs_rotated);
const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation};
calibrations[i].set_scale(accel_T_rotated.diag());
#if defined(DEBUD_BUILD)
PX4_INFO("accel %d: offset", i);
offset.print();
PX4_INFO("accel %d: bT * offset", i);
accel_offs_rotated.print();
PX4_INFO("accel %d: mat_A", i);
mat_A.print();
PX4_INFO("accel %d: accel_T", i);
accel_T.print();
PX4_INFO("accel %d: bT * accel_T * b", i);
accel_T_rotated.print();
#endif // DEBUD_BUILD
calibrations[i].PrintStatus();
if (calibrations[i].ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
}
if (!failed && factory_storage.store() != PX4_OK) {
failed = true;
}
if (param_save) {
param_notify_changes();
}
if (!failed) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
px4_usleep(600000); // give this message enough time to propagate
return PX4_OK;
}
}
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
px4_usleep(600000); // give this message enough time to propagate
return PX4_ERROR;
}
int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
{
#if !defined(CONSTRAINED_FLASH)
PX4_INFO("Accelerometer quick calibration");
bool param_save = false;
bool failed = true;
FactoryCalibrationStorage factory_storage;
if (factory_storage.open() != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
return PX4_ERROR;
}
// sensor thermal corrections (optional)
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
sensor_correction_s sensor_correction{};
sensor_correction_sub.copy(&sensor_correction);
uORB::SubscriptionMultiArray<sensor_accel_s, MAX_ACCEL_SENS> accel_subs{ORB_ID::sensor_accel};
/* use the first sensor to pace the readout, but do per-sensor counts */
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
sensor_accel_s arp{};
Vector3f accel_sum{};
unsigned count = 0;
while (accel_subs[accel_index].update(&arp)) {
// fetch optional thermal offset corrections in sensor/board frame
if ((arp.timestamp > 0) && (arp.device_id != 0)) {
Vector3f offset{0, 0, 0};
if (sensor_correction.timestamp > 0) {
for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) {
if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) {
switch (correction_index) {
case 0:
offset = Vector3f{sensor_correction.accel_offset_0};
break;
case 1:
offset = Vector3f{sensor_correction.accel_offset_1};
break;
case 2:
offset = Vector3f{sensor_correction.accel_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.accel_offset_3};
break;
}
}
}
}
const Vector3f accel{Vector3f{arp.x, arp.y, arp.z} - offset};
if (count > 0) {
const Vector3f diff{accel - (accel_sum / count)};
if (diff.norm() < 1.f) {
accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset;
count++;
}
} else {
accel_sum = accel;
count = 1;
}
}
}
if ((count > 0) && (arp.device_id != 0)) {
bool calibrated = false;
const Vector3f accel_avg = accel_sum / count;
Vector3f offset{0.f, 0.f, 0.f};
uORB::SubscriptionData<vehicle_attitude_s> attitude_sub{ORB_ID(vehicle_attitude)};
attitude_sub.update();
if (attitude_sub.advertised() && attitude_sub.get().timestamp != 0) {
// use vehicle_attitude if available
const vehicle_attitude_s &att = attitude_sub.get();
const matrix::Quatf q{att.q};
const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});
// sanity check angle between acceleration vectors
const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle();
if (angle <= math::radians(10.f)) {
offset = accel_avg - accel_ref;
calibrated = true;
}
}
if (!calibrated) {
// otherwise simply normalize to gravity and remove offset
Vector3f accel{accel_avg};
accel.normalize();
accel = accel * CONSTANTS_ONE_G;
offset = accel_avg - accel;
calibrated = true;
}
calibration::Accelerometer calibration{arp.device_id};
// reset cal index to uORB
calibration.set_calibration_index(accel_index);
if (!calibrated || (offset.norm() > CONSTANTS_ONE_G)
|| !PX4_ISFINITE(offset(0))
|| !PX4_ISFINITE(offset(1))
|| !PX4_ISFINITE(offset(2))) {
PX4_ERR("accel %d quick calibrate failed", accel_index);
} else {
calibration.set_offset(offset);
if (calibration.ParametersSave()) {
calibration.PrintStatus();
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "calibration save failed");
break;
}
}
}
}
if (!failed && factory_storage.store() != PX4_OK) {
failed = true;
}
if (param_save) {
param_notify_changes();
}
if (!failed) {
return PX4_OK;
}
#endif // !CONSTRAINED_FLASH
return PX4_ERROR;
}

51
src/prometheus_px4/src/modules/commander/accelerometer_calibration.h
Unescape View File

@ -1,51 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 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 accelerometer_calibration.h
*
* Definition of accelerometer calibration.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef ACCELEROMETER_CALIBRATION_H_
#define ACCELEROMETER_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_accel_calibration(orb_advert_t *mavlink_log_pub);
int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub);
#endif /* ACCELEROMETER_CALIBRATION_H_ */

292
src/prometheus_px4/src/modules/commander/airspeed_calibration.cpp
Unescape View File

@ -1,292 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 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 airspeed_calibration.cpp
* Airspeed sensor calibration routine
*/
#include "airspeed_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_airspeed.h>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>
static const char *sensor_name = "airspeed";
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
{
px4_sleep(5);
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
{
const hrt_abstime calibration_started = hrt_absolute_time();
int result = PX4_OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 2400;
/* give directions */
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
diff_pres_offset,
1.0f,
};
bool paramreset_successful = false;
int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
if (fd >= 0) {
if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
if (!paramreset_successful) {
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
}
calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
px4_usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
/* any differential pressure failure a reason to abort */
if (diff_pres.error_count != 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")",
diff_pres.error_count);
calibration_log_critical(mavlink_log_pub, "[cal] Check wiring, reboot vehicle, and try again");
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
if (calibration_counter % (calibration_count / 20) == 0) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (PX4_ISFINITE(diff_pres_offset)) {
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale >= 0) {
if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
// Prevent a completely zero param
// since this is used to detect a missing calibration
// This value is numerically down in the noise and has
// no effect on the sensor performance.
if (fabsf(diff_pres_offset) < 0.00000001f) {
diff_pres_offset = 0.00000001f;
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
px4_usleep(500 * 1000);
calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");
calibration_counter = 0;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
if (diff_pres.differential_pressure_filtered_pa > 0) {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
break;
} else {
/* do not allow negative values */
calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* save */
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
param_save_default();
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
tune_neutral(true);
}
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
/* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise
* the followup preflight checks might fail. */
px4_usleep(2e6);
normal_return:
px4_close(diff_pres_sub);
// This give a chance for the log messages to go out of the queue before someone else stomps on then
px4_sleep(1);
return result;
error_return:
result = PX4_ERROR;
goto normal_return;
}

47
src/prometheus_px4/src/modules/commander/airspeed_calibration.h
Unescape View File

@ -1,47 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gyro_calibration.h
* Airspeed sensor calibration routine
*/
#ifndef AIRSPEED_CALIBRATION_H_
#define AIRSPEED_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub);
#endif /* AIRSPEED_CALIBRATION_H_ */

68
src/prometheus_px4/src/modules/commander/calibration_messages.h
Unescape View File

@ -1,68 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 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 calibration_messages.h
*
* Common calibration messages.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef CALIBRATION_MESSAGES_H_
#define CALIBRATION_MESSAGES_H_
// The calibration message defines which begin with CAL_QGC_ are used by QGroundControl to run a state
// machine to provide visual feedback for calibration. As such, the text for them or semantics of when
// they are displayed cannot be modified without causing QGC to break. If modifications are made, make
// sure to bump the calibration version number which will cause QGC to perform log based calibration
// instead of visual calibration until such a time as QGC is update to the new version.
// The number in the cal started message is used to indicate the version stamp for the current calibration code.
#define CAL_QGC_STARTED_MSG "[cal] calibration started: 2 %s"
#define CAL_QGC_DONE_MSG "[cal] calibration done: %s"
#define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s"
// Warnings are deprecated because they were only used when it failed anyway.
//#define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s"
#define CAL_QGC_CANCELLED_MSG "[cal] calibration cancelled"
#define CAL_QGC_PROGRESS_MSG "[cal] progress <%u>"
#define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected"
#define CAL_QGC_SIDE_DONE_MSG "[cal] %s side done, rotate to a different side"
#define CAL_ERROR_SENSOR_MSG "[cal] calibration failed: reading sensor"
#define CAL_ERROR_RESET_CAL_MSG "[cal] calibration failed: to reset, sensor %u"
#define CAL_ERROR_READ_CAL_MSG "[cal] calibration failed: to read calibration"
#define CAL_ERROR_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration"
#define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters"
#endif /* CALIBRATION_MESSAGES_H_ */

367
src/prometheus_px4/src/modules/commander/calibration_routines.cpp
Unescape View File

@ -1,367 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2021 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 calibration_routines.cpp
* Calibration routines implementations.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/systemlib/mavlink_log.h>
#include <matrix/math.hpp>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include "calibration_routines.h"
#include "calibration_messages.h"
#include "commander_helper.h"
using namespace time_literals;
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, bool lenient_still_position)
{
static constexpr unsigned ndim = 3;
float accel_ema[ndim] {}; // exponential moving average of accel
float accel_disp[3] {}; // max-hold dispersion of accel
static constexpr float ema_len = 0.5f; // EMA time constant in seconds
static constexpr float normal_still_thr = 0.25; // normal still threshold
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
static constexpr float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
/* set timeout to 90s */
static constexpr hrt_abstime timeout = 90_s;
const hrt_abstime t_start = hrt_absolute_time();
hrt_abstime t_timeout = t_start + timeout;
hrt_abstime t = t_start;
hrt_abstime t_prev = t_start;
hrt_abstime t_still = 0;
unsigned poll_errcount = 0;
// Setup subscriptions to onboard accel sensor
uORB::SubscriptionBlocking<vehicle_acceleration_s> vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
while (true) {
vehicle_acceleration_s accel;
if (vehicle_acceleration_sub.updateBlocking(accel, 100000)) {
t = hrt_absolute_time();
float dt = (t - t_prev) / 1000000.0f;
t_prev = t;
float w = dt / ema_len;
for (unsigned i = 0; i < ndim; i++) {
float di = accel.xyz[i];
float d = di - accel_ema[i];
accel_ema[i] += d * w;
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
if (accel_disp[0] < still_thr2 &&
accel_disp[1] < still_thr2 &&
accel_disp[2] < still_thr2) {
/* is still now */
if (t_still == 0) {
/* first time */
calibration_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
} else {
/* still since t_still */
if (t > t_still + still_time) {
/* vehicle is still, exit from the loop to detection of its orientation */
break;
}
}
} else if (accel_disp[0] > still_thr2 * 4.0f ||
accel_disp[1] > still_thr2 * 4.0f ||
accel_disp[2] > still_thr2 * 4.0f) {
/* not still, reset still start time */
if (t_still != 0) {
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
px4_usleep(200000);
t_still = 0;
}
}
} else {
poll_errcount++;
}
if (t > t_timeout) {
poll_errcount++;
}
if (poll_errcount > 1000) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return ORIENTATION_ERROR;
}
}
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return ORIENTATION_LEFT; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return ORIENTATION_RIGHT; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
}
calibration_log_critical(mavlink_log_pub, "ERROR: invalid orientation");
return ORIENTATION_ERROR; // Can't detect orientation
}
const char *detect_orientation_str(enum detect_orientation_return orientation)
{
static const char *rgOrientationStrs[] = {
"back", // tail down
"front", // nose down
"left",
"right",
"up", // upside-down
"down", // right-side up
"error"
};
return rgOrientationStrs[orientation];
}
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker,
void *worker_data, bool lenient_still_position)
{
const hrt_abstime calibration_started = hrt_absolute_time();
calibrate_return result = calibrate_return_ok;
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[80];
pendingStr[0] = 0;
for (unsigned int cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strncat(pendingStr, " ", sizeof(pendingStr) - 1);
strncat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation), sizeof(pendingStr) - 1);
}
}
calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
px4_usleep(20000);
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
px4_usleep(20000);
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position);
if (orient == ORIENTATION_ERROR) {
orientation_failures++;
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
px4_usleep(20000);
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE);
calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient));
px4_usleep(20000);
continue;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
px4_usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
px4_usleep(20000);
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, worker_data);
if (result != calibrate_return_ok) {
break;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
px4_usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
px4_usleep(20000);
// Note that this side is complete
side_data_collected[orient] = true;
// output neutral tune
set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL);
// temporary priority boost for the white blinking led to come trough
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1);
px4_usleep(200000);
}
return result;
}
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started)
{
bool ret = false;
uORB::Subscription vehicle_command_sub{ORB_ID(vehicle_command)};
vehicle_command_s cmd;
while (vehicle_command_sub.update(&cmd)) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
// only handle commands sent after calibration started from external sources
if ((cmd.timestamp > calibration_started) && cmd.from_external) {
vehicle_command_ack_s command_ack{};
if ((int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 &&
(int)cmd.param3 == 0 &&
(int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) {
command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
tune_positive(true);
ret = true;
} else {
command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %" PRIu32, cmd.command);
tune_negative(true);
ret = false;
}
command_ack.command = cmd.command;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
command_ack.timestamp = hrt_absolute_time();
uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
command_ack_pub.publish(command_ack);
return ret;
}
}
}
return ret;
}

107
src/prometheus_px4/src/modules/commander/calibration_routines.h
Unescape View File

@ -1,107 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/// @file calibration_routines.h
/// @author Don Gagne <don@thegagnes.com>
#pragma once
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
// The order of these cannot change since the calibration calculations depend on them in this order
enum detect_orientation_return {
ORIENTATION_TAIL_DOWN,
ORIENTATION_NOSE_DOWN,
ORIENTATION_LEFT,
ORIENTATION_RIGHT,
ORIENTATION_UPSIDE_DOWN,
ORIENTATION_RIGHTSIDE_UP,
ORIENTATION_ERROR
};
static constexpr unsigned detect_orientation_side_count = 6;
/// Wait for vehicle to become still and detect it's orientation
/// @return Returns detect_orientation_return according to orientation when vehicle
/// and ready for measurements
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
bool lenient_still_detection); ///< true: Use more lenient still position detection
/// Returns the human readable string representation of the orientation
/// @param orientation Orientation to return string for, "error" if buffer is too small
const char *detect_orientation_str(enum detect_orientation_return orientation);
enum calibrate_return {
calibrate_return_ok,
calibrate_return_error,
calibrate_return_cancelled
};
typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return
orientation, ///< Orientation which was detected
void *worker_data); ///< Opaque worker data
/// Perform calibration sequence which require a rest orientation detection prior to calibration.
/// @return OK: Calibration succeeded, ERROR: Calibration failed
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration
calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration
void *worker_data, ///< Opaque data passed to worker routine
bool lenient_still_detection); ///< true: Use more lenient still position detection
/// Used to periodically check for a cancel command
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started);
// TODO FIXME: below are workarounds for QGC. The issue is that sometimes
// a mavlink log message is overwritten by the following one. A workaround
// is to wait for some time after publishing each message and hope that it
// gets sent out in the meantime.
#define calibration_log_info(_pub, _text, ...) \
do { \
mavlink_log_info(_pub, _text, ##__VA_ARGS__); \
px4_usleep(10000); \
} while(0);
#define calibration_log_critical(_pub, _text, ...) \
do { \
mavlink_log_critical(_pub, _text, ##__VA_ARGS__); \
px4_usleep(10000); \
} while(0);
#define calibration_log_emergency(_pub, _text, ...) \
do { \
mavlink_log_emergency(_pub, _text, ##__VA_ARGS__); \
px4_usleep(10000); \
} while(0);

371
src/prometheus_px4/src/modules/commander/commander_helper.cpp
Unescape View File

@ -1,371 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file commander_helper.cpp
* Commander helper functions implementations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
*/
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
#include <math.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/tune_control.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include "commander_helper.h"
#define VEHICLE_TYPE_FIXED_WING 1
#define VEHICLE_TYPE_QUADROTOR 2
#define VEHICLE_TYPE_COAXIAL 3
#define VEHICLE_TYPE_HELICOPTER 4
#define VEHICLE_TYPE_GROUND_ROVER 10
#define VEHICLE_TYPE_HEXAROTOR 13
#define VEHICLE_TYPE_OCTOROTOR 14
#define VEHICLE_TYPE_TRICOPTER 15
#define VEHICLE_TYPE_VTOL_DUOROTOR 19
#define VEHICLE_TYPE_VTOL_QUADROTOR 20
#define VEHICLE_TYPE_VTOL_TILTROTOR 21
#define VEHICLE_TYPE_VTOL_RESERVED2 22
#define VEHICLE_TYPE_VTOL_RESERVED3 23
#define VEHICLE_TYPE_VTOL_RESERVED4 24
#define VEHICLE_TYPE_VTOL_RESERVED5 25
#define BLINK_MSG_TIME 700000 // 3 fast blinks (in us)
bool is_multirotor(const vehicle_status_s &current_status)
{
return ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status.system_type == VEHICLE_TYPE_TRICOPTER));
}
bool is_rotary_wing(const vehicle_status_s &current_status)
{
return is_multirotor(current_status) || (current_status.system_type == VEHICLE_TYPE_HELICOPTER)
|| (current_status.system_type == VEHICLE_TYPE_COAXIAL);
}
bool is_vtol(const vehicle_status_s &current_status)
{
return (current_status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR ||
current_status.system_type == VEHICLE_TYPE_VTOL_TILTROTOR ||
current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED2 ||
current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED3 ||
current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED4 ||
current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED5);
}
bool is_vtol_tailsitter(const vehicle_status_s &current_status)
{
return (current_status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
}
bool is_fixed_wing(const vehicle_status_s &current_status)
{
return current_status.system_type == VEHICLE_TYPE_FIXED_WING;
}
bool is_ground_rover(const vehicle_status_s &current_status)
{
return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
}
static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message
static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence
static uint8_t tune_current = tune_control_s::TUNE_ID_STOP; // currently playing tune, can be interrupted after tune_end
static unsigned int tune_durations[tune_control_s::NUMBER_OF_TUNES] {};
static int fd_leds{-1};
static led_control_s led_control {};
static orb_advert_t led_control_pub = nullptr;
static tune_control_s tune_control {};
static orb_advert_t tune_control_pub = nullptr;
int buzzer_init()
{
tune_durations[tune_control_s::TUNE_ID_NOTIFY_POSITIVE] = 800000;
tune_durations[tune_control_s::TUNE_ID_NOTIFY_NEGATIVE] = 900000;
tune_durations[tune_control_s::TUNE_ID_NOTIFY_NEUTRAL] = 500000;
tune_durations[tune_control_s::TUNE_ID_ARMING_WARNING] = 3000000;
tune_durations[tune_control_s::TUNE_ID_HOME_SET] = 800000;
tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_FAST] = 800000;
tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW] = 800000;
tune_durations[tune_control_s::TUNE_ID_SINGLE_BEEP] = 300000;
tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH);
return PX4_OK;
}
void buzzer_deinit()
{
orb_unadvertise(tune_control_pub);
}
void set_tune_override(int tune)
{
tune_control.tune_id = tune;
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
tune_control.tune_override = true;
tune_control.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
}
void set_tune(int tune)
{
unsigned int new_tune_duration = tune_durations[tune];
/* don't interrupt currently playing non-repeating tune by repeating */
if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
/* allow interrupting current non-repeating tune by the same tune */
if (tune != tune_current || new_tune_duration != 0) {
tune_control.tune_id = tune;
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
tune_control.tune_override = false;
tune_control.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
}
tune_current = tune;
if (new_tune_duration != 0) {
tune_end = hrt_absolute_time() + new_tune_duration;
} else {
tune_end = 0;
}
}
}
void tune_home_set(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(tune_control_s::TUNE_ID_HOME_SET);
}
}
void tune_mission_ok(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
}
}
void tune_mission_warn(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color_and_mode(led_control_s::COLOR_YELLOW, led_control_s::MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL);
}
}
void tune_mission_fail(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color_and_mode(led_control_s::COLOR_RED, led_control_s::MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE);
}
}
/**
* Blink green LED and play positive tune (if use_buzzer == true).
*/
void tune_positive(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
}
}
/**
* Blink white LED and play neutral tune (if use_buzzer == true).
*/
void tune_neutral(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL);
}
}
/**
* Blink red LED and play negative tune (if use_buzzer == true).
*/
void tune_negative(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color_and_mode(led_control_s::COLOR_RED, led_control_s::MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE);
}
}
void tune_failsafe(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color_and_mode(led_control_s::COLOR_PURPLE, led_control_s::MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
}
}
int blink_msg_state()
{
if (blink_msg_end == 0) {
return 0;
} else if (hrt_absolute_time() > blink_msg_end) {
blink_msg_end = 0;
return 2;
} else {
return 1;
}
}
int led_init()
{
blink_msg_end = 0;
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_OFF;
led_control.priority = 0;
led_control.timestamp = hrt_absolute_time();
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
/* first open normal LEDs */
fd_leds = px4_open(LED0_DEVICE_PATH, O_RDWR);
if (fd_leds < 0) {
// there might not be an LED available, so don't make this an error
PX4_INFO("LED: open %s failed (%i)", LED0_DEVICE_PATH, errno);
return -errno;
}
/* the green LED is only available on FMUv5 */
px4_ioctl(fd_leds, LED_ON, LED_GREEN);
/* the blue LED is only available on AeroCore but not FMUv2 */
px4_ioctl(fd_leds, LED_ON, LED_BLUE);
/* switch blue off */
led_off(LED_BLUE);
/* we consider the amber led mandatory */
if (px4_ioctl(fd_leds, LED_ON, LED_AMBER)) {
PX4_WARN("Amber LED: ioctl fail");
return PX4_ERROR;
}
/* switch amber off */
led_off(LED_AMBER);
return 0;
}
void led_deinit()
{
orb_unadvertise(led_control_pub);
px4_close(fd_leds);
}
int led_toggle(int led)
{
return px4_ioctl(fd_leds, LED_TOGGLE, led);
}
int led_on(int led)
{
return px4_ioctl(fd_leds, LED_ON, led);
}
int led_off(int led)
{
return px4_ioctl(fd_leds, LED_OFF, led);
}
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)
{
led_control.mode = mode;
led_control.color = color;
led_control.num_blinks = blinks;
led_control.priority = prio;
led_control.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(led_control), led_control_pub, &led_control);
}
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode)
{
rgbled_set_color_and_mode(color, mode, 0, 0);
}

92
src/prometheus_px4/src/modules/commander/commander_helper.h
Unescape View File

@ -1,92 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file commander_helper.h
* Commander helper functions definitions
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef COMMANDER_HELPER_H_
#define COMMANDER_HELPER_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <drivers/drv_led.h>
#include <drivers/drv_board_led.h>
bool is_multirotor(const vehicle_status_s &current_status);
bool is_rotary_wing(const vehicle_status_s &current_status);
bool is_vtol(const vehicle_status_s &current_status);
bool is_vtol_tailsitter(const vehicle_status_s &current_status);
bool is_fixed_wing(const vehicle_status_s &current_status);
bool is_ground_rover(const vehicle_status_s &current_status);
int buzzer_init(void);
void buzzer_deinit(void);
void set_tune_override(int tune);
void set_tune(int tune);
void tune_home_set(bool use_buzzer);
void tune_mission_ok(bool use_buzzer);
void tune_mission_warn(bool use_buzzer);
void tune_mission_fail(bool use_buzzer);
void tune_positive(bool use_buzzer);
void tune_neutral(bool use_buzzer);
void tune_negative(bool use_buzzer);
void tune_failsafe(bool use_buzzer);
int blink_msg_state();
/* methods to control the onboard LED(s) */
int led_init(void);
void led_deinit(void);
int led_toggle(int led);
int led_on(int led);
int led_off(int led);
/**
* set the LED color & mode
* @param color @see led_control_s::COLOR_*
* @param mode @see led_control_s::MODE_*
*/
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode);
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio);
#endif /* COMMANDER_HELPER_H_ */

1029
src/prometheus_px4/src/modules/commander/commander_params.c
View File

File diff suppressed because it is too large Load Diff

41
src/prometheus_px4/src/modules/commander/commander_tests/CMakeLists.txt
Unescape View File

@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__commander__commander_tests
MAIN commander_tests
SRCS
commander_tests.cpp
state_machine_helper_test.cpp
../state_machine_helper.cpp
DEPENDS
)

52
src/prometheus_px4/src/modules/commander/commander_tests/commander_tests.cpp
Unescape View File

@ -1,52 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* 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 commander_tests.cpp
* Commander unit tests. Run the tests as follows:
* nsh> commander_tests
*
*/
#include <systemlib/err.h>
#include "state_machine_helper_test.h"
extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
int commander_tests_main(int argc, char *argv[])
{
return stateMachineHelperTest() ? 0 : -1;
}

546
src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.cpp
Unescape View File

@ -1,546 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* 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 state_machine_helper_test.cpp
* System state machine unit test.
*
*/
#include "state_machine_helper_test.h"
#include "../state_machine_helper.h"
#include <unit_test.h>
#include "../Arming/PreFlightCheck/PreFlightCheck.hpp"
class StateMachineHelperTest : public UnitTest
{
public:
StateMachineHelperTest() = default;
~StateMachineHelperTest() override = default;
bool run_tests() override;
private:
bool armingStateTransitionTest();
bool mainStateTransitionTest();
};
bool StateMachineHelperTest::armingStateTransitionTest()
{
// These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
// to simulate machine state prior to testing an arming state transition. This structure is also
// use to represent the expected machine state after the transition has been requested.
typedef struct {
arming_state_t arming_state; // vehicle_status_s.arming_state
bool armed; // actuator_armed_s.armed
bool ready_to_arm; // actuator_armed_s.ready_to_arm
} ArmingTransitionVolatileState_t;
// This structure represents a test case for arming_state_transition. It contains the machine
// state prior to transition, the requested state to transition to and finally the expected
// machine state after transition.
typedef struct {
const char *assertMsg; // Text to show when test case fails
ArmingTransitionVolatileState_t current_state; // Machine state prior to transition
hil_state_t hil_state; // Current vehicle_status_s.hil_state
bool
condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized
bool safety_switch_available; // Current safety_s.safety_switch_available
bool safety_off; // Current safety_s.safety_off
arming_state_t requested_state; // Requested arming state to transition to
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
transition_result_t expected_transition_result; // Expected result from arming_state_transition
} ArmingTransitionTest_t;
// We use these defines so that our test cases are more readable
#define ATT_ARMED true
#define ATT_DISARMED false
#define ATT_READY_TO_ARM true
#define ATT_NOT_READY_TO_ARM false
#define ATT_SENSORS_INITIALIZED true
#define ATT_SENSORS_NOT_INITIALIZED false
#define ATT_SAFETY_AVAILABLE true
#define ATT_SAFETY_NOT_AVAILABLE true
#define ATT_SAFETY_OFF true
#define ATT_SAFETY_ON false
// These are test cases for arming_state_transition
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{
"no transition: identical states",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED
},
// TRANSITION_CHANGED tests
// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
{
"transition: init to standby",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: init to standby error",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: init to reboot",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_SHUTDOWN,
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: standby to init",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: standby to standby error",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: standby to reboot",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_SHUTDOWN,
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: armed to standby",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: standby error to reboot",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_SHUTDOWN,
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: in air restore to armed",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: in air restore to reboot",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_SHUTDOWN,
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
// hil on tests, standby error to standby not normally allowed
{
"transition: standby error to standby, hil on",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
// Safety switch arming tests
{
"transition: standby to armed, no safety switch",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{
"transition: standby to armed, safety switch off",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
// TRANSITION_DENIED tests
// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
{
"no transition: init to armed",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{
"no transition: armed to init",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
{
"no transition: armed to reboot",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_SHUTDOWN,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
{
"no transition: standby error to armed",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{
"no transition: standby error to standby",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{
"no transition: reboot to armed",
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{
"no transition: in air restore to standby",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
// Sensor tests
//{ "transition to standby error: init to standby - sensors not initialized",
// { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
// vehicle_status_s::ARMING_STATE_STANDBY,
// { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Safety switch arming tests
{
"no transition: init to armed, safety switch on",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
};
struct vehicle_status_s status = {};
struct vehicle_status_flags_s status_flags = {};
struct safety_s safety = {};
struct actuator_armed_s armed = {};
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i = 0; i < cArmingTransitionTests; i++) {
const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i];
PreFlightCheck::arm_requirements_t arm_req{};
// Setup initial machine state
status.arming_state = test->current_state.arming_state;
status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
status.hil_state = test->hil_state;
// The power status of the test unit is not relevant for the unit test
status_flags.circuit_breaker_engaged_power_check = true;
safety.safety_switch_available = test->safety_switch_available;
safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(status, safety, test->requested_state, armed,
true /* enable pre-arm checks */,
nullptr /* no mavlink_log_pub */,
status_flags,
arm_req,
2e6, /* 2 seconds after boot, everything should be checked */
arm_disarm_reason_t::UNIT_TEST);
// Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result);
ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state);
ut_compare(test->assertMsg, armed.armed, test->expected_state.armed);
ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm);
}
return true;
}
bool StateMachineHelperTest::mainStateTransitionTest()
{
// This structure represent a single test case for testing Main State transitions.
typedef struct {
const char *assertMsg; // Text to show when test case fails
uint8_t condition_bits; // Bits for various condition_* values
uint8_t from_state; // State prior to transition request
main_state_t to_state; // State to transition to
transition_result_t expected_transition_result; // Expected result from main_state_transition call
} MainTransitionTest_t;
// Bits for condition_bits
#define MTT_ALL_NOT_VALID 0
#define MTT_ROTARY_WING 1 << 0
#define MTT_LOC_ALT_VALID 1 << 1
#define MTT_LOC_POS_VALID 1 << 2
#define MTT_HOME_POS_VALID 1 << 3
#define MTT_GLOBAL_POS_VALID 1 << 4
static const MainTransitionTest_t rgMainTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{
"no transition: identical states",
MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED
},
// TRANSITION_CHANGED tests
{
"transition: MANUAL to ACRO - rotary",
MTT_ROTARY_WING,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED
},
{
"transition: MANUAL to ACRO - not rotary",
MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED
},
{
"transition: ACRO to MANUAL",
MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{
"transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED
},
{
"transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{
"transition: MANUAL to AUTO_LOITER - global position valid",
MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED
},
{
"transition: AUTO_LOITER to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{
"transition: MANUAL to AUTO_RTL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED
},
{
"transition: AUTO_RTL to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{
"transition: MANUAL to ALTCTL - not rotary",
MTT_LOC_ALT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
},
{
"transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
},
{
"transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
},
{
"transition: ALTCTL to MANUAL - local altitude valid",
MTT_LOC_ALT_VALID,
commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{
"transition: MANUAL to POSCTL - local position not valid, global position valid",
MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED
},
{
"transition: MANUAL to POSCTL - local position valid, global position not valid",
MTT_LOC_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED
},
{
"transition: POSCTL to MANUAL - local position valid, global position valid",
MTT_LOC_POS_VALID,
commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
// TRANSITION_DENIED tests
{
"no transition: MANUAL to AUTO_MISSION - global position not valid",
MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED
},
{
"no transition: MANUAL to AUTO_LOITER - global position not valid",
MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED
},
{
"no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
},
{
"no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
},
{
"no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
},
{
"transition: MANUAL to ALTCTL - not rotary",
MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED
},
{
"no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
MTT_ROTARY_WING,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED
},
{
"no transition: MANUAL to POSCTL - local position not valid, global position not valid",
MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED
},
};
size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
for (size_t i = 0; i < cMainTransitionTests; i++) {
const MainTransitionTest_t *test = &rgMainTransitionTests[i];
// Setup initial machine state
struct vehicle_status_s current_vehicle_status = {};
struct commander_state_s current_commander_state = {};
struct vehicle_status_flags_s current_status_flags = {};
current_commander_state.main_state = test->from_state;
current_vehicle_status.vehicle_type = (test->condition_bits & MTT_ROTARY_WING) ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
current_status_flags.condition_auto_mission_available = true;
// Attempt transition
transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags,
current_commander_state);
// Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result);
if (test->expected_transition_result == result) {
if (test->expected_transition_result == TRANSITION_CHANGED) {
ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state);
} else {
ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state);
}
}
}
return true;
}
bool StateMachineHelperTest::run_tests()
{
ut_run_test(armingStateTransitionTest);
ut_run_test(mainStateTransitionTest);
return (_tests_failed == 0);
}
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)

42
src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.h
Unescape View File

@ -1,42 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* 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 state_machine_helper_test.h
*/
#pragma once
bool stateMachineHelperTest(void);

177
src/prometheus_px4/src/modules/commander/esc_calibration.cpp
Unescape View File

@ -1,177 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015-2017 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 esc_calibration.cpp
*
* Definition of esc calibration
*
* @author Roman Bapst <roman@px4.io>
*/
#include "esc_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
using namespace time_literals;
bool check_battery_disconnected(orb_advert_t *mavlink_log_pub)
{
uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
const battery_status_s &battery = batt_sub.get();
batt_sub.update();
if (battery.timestamp == 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "battery unavailable");
return false;
}
// Make sure battery is disconnected
// battery is not connected if the connected flag is not set and we have a recent battery measurement
if (!battery.connected && (hrt_elapsed_time(&battery.timestamp) < 500_ms)) {
return true;
}
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
return false;
}
int do_esc_calibration(orb_advert_t *mavlink_log_pub)
{
int return_code = PX4_OK;
hrt_abstime timeout_start = 0;
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
const battery_status_s &battery = batt_sub.get();
batt_sub.update();
bool batt_connected = battery.connected;
int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
return_code = PX4_ERROR;
goto Out;
}
/* tell IO/FMU that its ok to disable its safety with the switch */
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
return_code = PX4_ERROR;
goto Out;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
return_code = PX4_ERROR;
goto Out;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
return_code = PX4_ERROR;
goto Out;
}
calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
timeout_start = hrt_absolute_time();
while (true) {
// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
// sit high.
static constexpr hrt_abstime battery_connect_wait_timeout{20_s};
static constexpr hrt_abstime pwm_high_timeout{3_s};
hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
if (hrt_elapsed_time(&timeout_start) > timeout_wait) {
if (!batt_connected) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
return_code = PX4_ERROR;
goto Out;
}
// PWM was high long enough
break;
}
if (!batt_connected) {
if (batt_sub.update()) {
if (battery.connected) {
// Battery is connected, signal to user and start waiting again
batt_connected = true;
timeout_start = hrt_absolute_time();
calibration_log_info(mavlink_log_pub, "[cal] Battery connected");
}
}
}
px4_usleep(50000);
}
Out:
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
}
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
}
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated");
}
px4_close(fd);
}
if (return_code == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
return return_code;
}

51
src/prometheus_px4/src/modules/commander/esc_calibration.h
Unescape View File

@ -1,51 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file esc_calibration.h
*
* Definition of esc calibration
*
* @author Roman Bapst <roman@px4.io>
*/
#ifndef ESC_CALIBRATION_H_
#define ESC_CALIBRATION_H_
#include <uORB/topics/actuator_armed.h>
int do_esc_calibration(orb_advert_t *mavlink_log_pub);
bool check_battery_disconnected(orb_advert_t *mavlink_log_pub);
#endif

107
src/prometheus_px4/src/modules/commander/factory_calibration_storage.cpp
Unescape View File

@ -1,107 +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.
*
****************************************************************************/
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <lib/parameters/param.h>
#include <px4_platform_common/log.h>
#include "factory_calibration_storage.h"
static const char *CALIBRATION_STORAGE = "/fs/mtd_caldata";
static bool filter_calibration_params(param_t handle)
{
const char *name = param_name(handle);
// filter all non-calibration params
return strncmp(name, "CAL_", 4) == 0 || strncmp(name, "TC_", 3) == 0;
}
FactoryCalibrationStorage::FactoryCalibrationStorage()
{
int32_t param = 0;
param_get(param_find("SYS_FAC_CAL_MODE"), &param);
_enabled = param == 1;
}
int FactoryCalibrationStorage::open()
{
if (_fd >= 0) {
cleanup();
}
if (!_enabled) {
return 0;
}
_fd = ::open(CALIBRATION_STORAGE, O_RDWR);
if (_fd == -1) {
return -errno;
}
PX4_INFO("Storing parameters to factory storage %s", CALIBRATION_STORAGE);
param_control_autosave(false);
return 0;
}
int FactoryCalibrationStorage::store()
{
if (!_enabled) {
return 0;
}
int ret = param_export(_fd, false, filter_calibration_params);
if (ret != 0) {
PX4_ERR("param export failed (%i)", ret);
}
return ret;
}
void FactoryCalibrationStorage::cleanup()
{
if (_enabled) {
param_control_autosave(true);
}
if (_fd >= 0) {
close(_fd);
_fd = -1;
}
}

64
src/prometheus_px4/src/modules/commander/factory_calibration_storage.h
Unescape View File

@ -1,64 +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.
*
****************************************************************************/
#pragma once
/**
* @class FactoryCalibrationStorage
* Stores calibration parameters to a separate storage, if enabled by parameter
*/
class FactoryCalibrationStorage
{
public:
FactoryCalibrationStorage();
~FactoryCalibrationStorage() { cleanup(); }
/**
* open the storage & disable param autosaving
* @return 0 on success, <0 error otherwise
*/
int open();
/**
* store the calibration parameters
* Note: this method requires a lot of stack
* @return 0 on success, <0 error otherwise
*/
int store();
private:
void cleanup();
bool _enabled{false};
int _fd{-1};
};

36
src/prometheus_px4/src/modules/commander/failure_detector/CMakeLists.txt
Unescape View File

@ -1,36 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(failure_detector
FailureDetector.cpp
)

157
src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.cpp
Unescape View File

@ -1,157 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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 FailureDetector.cpp
*
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
*
*/
#include "FailureDetector.hpp"
using namespace time_literals;
FailureDetector::FailureDetector(ModuleParams *parent) :
ModuleParams(parent)
{
}
bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode)
{
uint8_t previous_status = _status;
if (vehicle_control_mode.flag_control_attitude_enabled) {
updateAttitudeStatus();
if (_param_fd_ext_ats_en.get()) {
updateExternalAtsStatus();
}
} else {
_status &= ~(FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT);
}
if (_param_escs_en.get()) {
updateEscsStatus(vehicle_status);
}
return _status != previous_status;
}
void FailureDetector::updateAttitudeStatus()
{
vehicle_attitude_s attitude;
if (_vehicule_attitude_sub.update(&attitude)) {
const matrix::Eulerf euler(matrix::Quatf(attitude.q));
const float roll(euler.phi());
const float pitch(euler.theta());
const float max_roll_deg = _param_fd_fail_r.get();
const float max_pitch_deg = _param_fd_fail_p.get();
const float max_roll(fabsf(math::radians(max_roll_deg)));
const float max_pitch(fabsf(math::radians(max_pitch_deg)));
const bool roll_status = (max_roll > 0.0f) && (fabsf(roll) > max_roll);
const bool pitch_status = (max_pitch > 0.0f) && (fabsf(pitch) > max_pitch);
hrt_abstime time_now = hrt_absolute_time();
// Update hysteresis
_roll_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_r_ttri.get()));
_pitch_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_p_ttri.get()));
_roll_failure_hysteresis.set_state_and_update(roll_status, time_now);
_pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now);
// Update bitmask
_status &= ~(FAILURE_ROLL | FAILURE_PITCH);
if (_roll_failure_hysteresis.get_state()) {
_status |= FAILURE_ROLL;
}
if (_pitch_failure_hysteresis.get_state()) {
_status |= FAILURE_PITCH;
}
}
}
void FailureDetector::updateExternalAtsStatus()
{
pwm_input_s pwm_input;
if (_pwm_input_sub.update(&pwm_input)) {
uint32_t pulse_width = pwm_input.pulse_width;
bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms);
hrt_abstime time_now = hrt_absolute_time();
// Update hysteresis
_ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz
_ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now);
_status &= ~FAILURE_EXT;
if (_ext_ats_failure_hysteresis.get_state()) {
_status |= FAILURE_EXT;
}
}
}
void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
{
hrt_abstime time_now = hrt_absolute_time();
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
esc_status_s esc_status;
if (_esc_status_sub.update(&esc_status)) {
int all_escs_armed = (1 << esc_status.esc_count) - 1;
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
if (_esc_failure_hysteresis.get_state()) {
_status |= FAILURE_ARM_ESCS;
}
}
} else {
// reset ESC bitfield
_esc_failure_hysteresis.set_state_and_update(false, time_now);
_status &= ~FAILURE_ARM_ESCS;
}
}

104
src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.hpp
Unescape View File

@ -1,104 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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 FailureDetector.hpp
* Base class for failure detection logic based on vehicle states
* for failsafe triggering.
*
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
*
*/
#pragma once
#include <matrix/matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <px4_platform_common/module_params.h>
#include <hysteresis/hysteresis.h>
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/pwm_input.h>
typedef enum {
FAILURE_NONE = vehicle_status_s::FAILURE_NONE,
FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL,
FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH,
FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
FAILURE_EXT = vehicle_status_s::FAILURE_EXT,
FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC
} failure_detector_bitmak;
using uORB::SubscriptionData;
class FailureDetector : public ModuleParams
{
public:
FailureDetector(ModuleParams *parent);
bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
uint8_t getStatus() const { return _status; }
private:
void updateAttitudeStatus();
void updateExternalAtsStatus();
void updateEscsStatus(const vehicle_status_s &vehicle_status);
uint8_t _status{FAILURE_NONE};
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};
systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
systemlib::Hysteresis _esc_failure_hysteresis{false};
uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r,
(ParamFloat<px4::params::FD_FAIL_R_TTRI>) _param_fd_fail_r_ttri,
(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en
)
};

144
src/prometheus_px4/src/modules/commander/failure_detector/failure_detector_params.c
Unescape View File

@ -1,144 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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 failure_detector_params.c
*
* Parameters used by the Failure Detector.
*
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
*/
#include <px4_platform_common/px4_config.h>
#include <parameters/param.h>
/**
* FailureDetector Max Roll
*
* Maximum roll angle before FailureDetector triggers the attitude_failure flag.
* The flag triggers flight termination (if @CBRK_FLIGHTTERM = 0),
* which sets outputs to their failsafe values.
* On takeoff the flag triggers lockdown (irrespective of @CBRK_FLIGHTTERM),
* which disarms motors but does not set outputs to failsafe values.
*
* Setting this parameter to 0 disables the check
*
* @min 60
* @max 180
* @unit deg
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_FAIL_R, 60);
/**
* FailureDetector Max Pitch
*
* Maximum pitch angle before FailureDetector triggers the attitude_failure flag.
* The flag triggers flight termination (if @CBRK_FLIGHTTERM = 0),
* which sets outputs to their failsafe values.
* On takeoff the flag triggers lockdown (irrespective of @CBRK_FLIGHTTERM),
* which disarms motors but does not set outputs to failsafe values.
*
* Setting this parameter to 0 disables the check
*
* @min 60
* @max 180
* @unit deg
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_FAIL_P, 60);
/**
* Roll failure trigger time
*
* Seconds (decimal) that roll has to exceed FD_FAIL_R before being considered as a failure.
*
* @unit s
* @min 0.02
* @max 5
* @decimal 2
*
* @group Failure Detector
*/
PARAM_DEFINE_FLOAT(FD_FAIL_R_TTRI, 0.3);
/**
* Pitch failure trigger time
*
* Seconds (decimal) that pitch has to exceed FD_FAIL_P before being considered as a failure.
*
* @unit s
* @min 0.02
* @max 5
* @decimal 2
*
* @group Failure Detector
*/
PARAM_DEFINE_FLOAT(FD_FAIL_P_TTRI, 0.3);
/**
* Enable PWM input on for engaging failsafe from an external automatic trigger system (ATS).
*
* Enabled on either AUX5 or MAIN5 depending on board.
* External ATS is required by ASTM F3322-18.
*
* @boolean
* @reboot_required true
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0);
/**
* The PWM threshold from external automatic trigger system for engaging failsafe.
*
* External ATS is required by ASTM F3322-18.
*
* @unit us
* @decimal 2
*
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
/**
* Enable checks on ESCs that report their arming state.
*
* If enabled, failure detector will verify that all the ESCs have successfully armed when the vehicle has transitioned to the armed state.
* Timeout for receiving an acknowledgement from the ESCs is 0.3s, if no feedback is received the failure detector will auto disarm the vehicle.
*
* @boolean
* @reboot_required true
*
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_ESCS_EN, 1);

298
src/prometheus_px4/src/modules/commander/gyro_calibration.cpp
Unescape View File

@ -1,298 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-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 gyro_calibration.cpp
*
* Gyroscope calibration routine
*/
#include <px4_platform_common/px4_config.h>
#include "factory_calibration_storage.h"
#include "gyro_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_platform_common/posix.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/time.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/math/filter/MedianFilter.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/sensor_gyro.h>
static constexpr char sensor_name[] {"gyro"};
static constexpr unsigned MAX_GYROS = 4;
using matrix::Vector3f;
/// Data passed to calibration worker routine
struct gyro_worker_data_t {
orb_advert_t *mavlink_log_pub{nullptr};
calibration::Gyroscope calibrations[MAX_GYROS] {};
Vector3f offset[MAX_GYROS] {};
math::MedianFilter<float, 9> filter[3] {};
};
static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
{
const hrt_abstime calibration_started = hrt_absolute_time();
unsigned calibration_counter[MAX_GYROS] {};
static constexpr unsigned CALIBRATION_COUNT = 250;
unsigned poll_errcount = 0;
uORB::SubscriptionBlocking<sensor_gyro_s> gyro_sub[MAX_GYROS] {
{ORB_ID(sensor_gyro), 0, 0},
{ORB_ID(sensor_gyro), 0, 1},
{ORB_ID(sensor_gyro), 0, 2},
{ORB_ID(sensor_gyro), 0, 3},
};
/* use slowest gyro to pace, but count correctly per-gyro for statistics */
unsigned slow_count = 0;
while (slow_count < CALIBRATION_COUNT) {
if (calibrate_cancel_check(worker_data.mavlink_log_pub, calibration_started)) {
return calibrate_return_cancelled;
}
if (gyro_sub[0].updatedBlocking(100000)) {
unsigned update_count = CALIBRATION_COUNT;
for (unsigned gyro_index = 0; gyro_index < MAX_GYROS; gyro_index++) {
if (worker_data.calibrations[gyro_index].device_id() != 0) {
if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) {
// Skip if instance has enough samples
continue;
}
sensor_gyro_s gyro_report;
while (gyro_sub[gyro_index].update(&gyro_report)) {
// fetch optional thermal offset corrections in sensor frame
const Vector3f &thermal_offset{worker_data.calibrations[gyro_index].thermal_offset()};
worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - thermal_offset;
calibration_counter[gyro_index]++;
if (gyro_index == 0) {
worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
worker_data.filter[2].insert(gyro_report.z - thermal_offset(2));
}
}
// Maintain the sample count of the slowest sensor
if (calibration_counter[gyro_index] && calibration_counter[gyro_index] < update_count) {
update_count = calibration_counter[gyro_index];
}
}
}
if (update_count % (CALIBRATION_COUNT / 20) == 0) {
calibration_log_info(worker_data.mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / CALIBRATION_COUNT);
}
// Propagate out the slowest sensor's count
if (slow_count < update_count) {
slow_count = update_count;
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
calibration_log_critical(worker_data.mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return calibrate_return_error;
}
}
for (unsigned s = 0; s < MAX_GYROS; s++) {
if ((worker_data.calibrations[s].device_id() != 0) && (calibration_counter[s] < CALIBRATION_COUNT / 2)) {
calibration_log_critical(worker_data.mavlink_log_pub, "ERROR: missing data, sensor %d", s)
return calibrate_return_error;
}
worker_data.offset[s] /= calibration_counter[s];
}
return calibrate_return_ok;
}
int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
{
int res = PX4_OK;
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
gyro_worker_data_t worker_data{};
worker_data.mavlink_log_pub = mavlink_log_pub;
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_gyro));
// Warn that we will not calibrate more than MAX_GYROS gyroscopes
if (orb_gyro_count > MAX_GYROS) {
calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, MAX_GYROS);
} else if (orb_gyro_count < 1) {
calibration_log_critical(mavlink_log_pub, "No gyros found");
return PX4_ERROR;
}
for (uint8_t cur_gyro = 0; cur_gyro < MAX_GYROS; cur_gyro++) {
uORB::SubscriptionData<sensor_gyro_s> gyro_sub{ORB_ID(sensor_gyro), cur_gyro};
if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) {
worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id);
}
// reset calibration index to match uORB numbering
worker_data.calibrations[cur_gyro].set_calibration_index(cur_gyro);
}
unsigned try_count = 0;
unsigned max_tries = 20;
res = PX4_ERROR;
do {
// Calibrate gyro and ensure user didn't move
calibrate_return cal_return = gyro_calibration_worker(worker_data);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already sent, we are done here
res = PX4_ERROR;
break;
} else if (cal_return == calibrate_return_error) {
res = PX4_ERROR;
} else {
/* check offsets using a median filter */
float xdiff = worker_data.filter[0].median() - worker_data.offset[0](0);
float ydiff = worker_data.filter[1].median() - worker_data.offset[0](1);
float zdiff = worker_data.filter[2].median() - worker_data.offset[0](2);
/* maximum allowable calibration error */
static constexpr float maxoff = math::radians(0.6f);
if (!PX4_ISFINITE(worker_data.offset[0](0)) ||
!PX4_ISFINITE(worker_data.offset[0](1)) ||
!PX4_ISFINITE(worker_data.offset[0](2)) ||
fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) {
calibration_log_critical(mavlink_log_pub, "motion, retrying..");
res = PX4_ERROR;
} else {
res = PX4_OK;
}
}
try_count++;
} while (res == PX4_ERROR && try_count <= max_tries);
if (try_count >= max_tries) {
calibration_log_critical(mavlink_log_pub, "ERROR: Motion during calibration");
res = PX4_ERROR;
}
FactoryCalibrationStorage factory_storage;
if (factory_storage.open() != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
res = PX4_ERROR;
}
if (res == PX4_OK) {
// set offset parameters to new values
bool param_save = false;
bool failed = true;
for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) {
auto &calibration = worker_data.calibrations[uorb_index];
if (calibration.device_id() != 0) {
calibration.set_offset(worker_data.offset[uorb_index]);
calibration.set_calibration_index(uorb_index);
calibration.PrintStatus();
if (calibration.ParametersSave()) {
param_save = true;
failed = false;
} else {
failed = true;
calibration_log_critical(mavlink_log_pub, "calibration save failed");
break;
}
}
}
if (!failed && factory_storage.store() != PX4_OK) {
failed = true;
}
if (param_save) {
param_notify_changes();
}
if (!failed) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
px4_usleep(600000); // give this message enough time to propagate
return PX4_OK;
}
}
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
px4_usleep(600000); // give this message enough time to propagate
return PX4_ERROR;
}

47
src/prometheus_px4/src/modules/commander/gyro_calibration.h
Unescape View File

@ -1,47 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 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 gyro_calibration.h
* Gyroscope calibration routine
*/
#ifndef GYRO_CALIBRATION_H_
#define GYRO_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_gyro_calibration(orb_advert_t *mavlink_log_pub);
#endif /* GYRO_CALIBRATION_H_ */

186
src/prometheus_px4/src/modules/commander/level_calibration.cpp
Unescape View File

@ -1,186 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-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.
*
****************************************************************************/
#include "level_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/ecl/geo/geo.h>
#include <matrix/math.hpp>
#include <lib/conversion/rotation.h>
#include <lib/parameters/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
using namespace time_literals;
using namespace matrix;
using math::radians;
int do_level_calibration(orb_advert_t *mavlink_log_pub)
{
bool success = false;
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");
param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
// get old values
float roll_offset_current = 0.f;
float pitch_offset_current = 0.f;
param_get(roll_offset_handle, &roll_offset_current);
param_get(pitch_offset_handle, &pitch_offset_current);
int32_t board_rot_current = 0;
param_get(param_find("SENS_BOARD_ROT"), &board_rot_current);
const Dcmf board_rotation_offset{Eulerf{radians(roll_offset_current), radians(pitch_offset_current), 0.f}};
float roll_mean = 0.f;
float pitch_mean = 0.f;
unsigned counter = 0;
bool had_motion = true;
int num_retries = 0;
uORB::SubscriptionBlocking<vehicle_attitude_s> att_sub{ORB_ID(vehicle_attitude)};
while (had_motion && num_retries++ < 50) {
Vector2f min_angles{100.f, 100.f};
Vector2f max_angles{-100.f, -100.f};
roll_mean = 0.0f;
pitch_mean = 0.0f;
counter = 0;
int last_progress_report = -100;
static constexpr hrt_abstime calibration_duration = 500_ms;
const hrt_abstime start = hrt_absolute_time();
while (hrt_elapsed_time(&start) < calibration_duration) {
vehicle_attitude_s att{};
if (!att_sub.updateBlocking(att, 100000)) {
// attitude estimator is not running
calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
goto out;
}
int progress = 100 * hrt_elapsed_time(&start) / calibration_duration;
if (progress >= last_progress_report + 20) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress);
last_progress_report = progress;
}
Eulerf att_euler{Quatf{att.q}};
// keep min + max angles
for (int i = 0; i < 2; ++i) {
if (att_euler(i) < min_angles(i)) { min_angles(i) = att_euler(i); }
if (att_euler(i) > max_angles(i)) { max_angles(i) = att_euler(i); }
}
att_euler(2) = 0.f; // ignore yaw
att_euler = Eulerf{board_rotation_offset *Dcmf{att_euler}}; // subtract existing board rotation
roll_mean += att_euler.phi();
pitch_mean += att_euler.theta();
++counter;
}
// motion detection: check that (max-min angle) is within a threshold.
// The difference is typically <0.1 deg while at rest
if (max_angles(0) - min_angles(0) < math::radians(0.5f) &&
max_angles(1) - min_angles(1) < math::radians(0.5f)) {
had_motion = false;
}
}
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
roll_mean /= counter;
pitch_mean /= counter;
if (had_motion) {
calibration_log_critical(mavlink_log_pub, "motion during calibration");
} else if (fabsf(roll_mean) > 0.8f) {
calibration_log_critical(mavlink_log_pub, "excess roll angle");
} else if (fabsf(pitch_mean) > 0.8f) {
calibration_log_critical(mavlink_log_pub, "excess pitch angle");
} else {
float roll_mean_degrees = math::degrees(roll_mean);
float pitch_mean_degrees = math::degrees(pitch_mean);
if (fabsf(roll_offset_current - roll_mean_degrees) > 0.1f) {
PX4_INFO("Updating SENS_BOARD_X_OFF %.1f -> %.1f degrees", (double)roll_offset_current, (double)roll_mean_degrees);
}
if (fabsf(pitch_offset_current - pitch_mean_degrees) > 0.1f) {
PX4_INFO("Updating SENS_BOARD_Y_OFF %.1f -> %.1f degrees", (double)pitch_offset_current, (double)pitch_mean_degrees);
}
param_set_no_notification(roll_offset_handle, &roll_mean_degrees);
param_set_no_notification(pitch_offset_handle, &pitch_mean_degrees);
param_notify_changes();
success = true;
}
out:
if (success) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
px4_usleep(600000); // give this message enough time to propagate
return 0;
} else {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
px4_usleep(600000); // give this message enough time to propagate
return 1;
}
}

39
src/prometheus_px4/src/modules/commander/level_calibration.h
Unescape View File

@ -1,39 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-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.
*
****************************************************************************/
#pragma once
#include <stdint.h>
#include <uORB/uORB.h>
int do_level_calibration(orb_advert_t *mavlink_log_pub);

356
src/prometheus_px4/src/modules/commander/lm_fit.cpp
Unescape View File

@ -1,356 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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.
*
****************************************************************************/
#include "lm_fit.hpp"
struct iteration_result {
float gradient_damping;
float cost;
enum class STATUS {
SUCCESS, FAILURE
} result = STATUS::SUCCESS;
};
void lm_sphere_fit_iteration(const float x[], const float y[], const float z[],
unsigned int samples_collected, sphere_params &params, iteration_result &result)
{
// Run Sphere Fit using Levenberg Marquardt LSq Fit
const float lma_damping = 10.f;
float fitness = result.cost;
float fit1 = 0.f;
float fit2 = 0.f;
matrix::SquareMatrix<float, 4> JTJ;
float JTFI[4] {};
float residual = 0.0f;
// Gauss Newton Part common for all kind of extensions including LM
for (uint16_t k = 0; k < samples_collected; k++) {
float sphere_jacob[4];
//Calculate Jacobian
float A = (params.diag(0) * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) +
(params.offdiag(1) * (z[k] - params.offset(2)));
float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1) * (y[k] - params.offset(1))) +
(params.offdiag(2) * (z[k] - params.offset(2)));
float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) +
(params.diag(2) * (z[k] - params.offset(2)));
float length = sqrtf(A * A + B * B + C * C);
// 0: partial derivative (radius wrt fitness fn) fn operated on sample
sphere_jacob[0] = 1.0f;
// 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
sphere_jacob[1] = 1.0f * (((params.diag(0) * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length);
sphere_jacob[2] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1) * B) + (params.offdiag(2) * C)) / length);
sphere_jacob[3] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2) * C)) / length);
residual = params.radius - length;
for (uint8_t i = 0; i < 4; i++) {
// compute JTJ
for (uint8_t j = 0; j < 4; j++) {
JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j];
}
JTFI[i] += sphere_jacob[i] * residual;
}
}
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
float fit1_params[4] = {params.radius, params.offset(0), params.offset(1), params.offset(2)};
float fit2_params[4];
memcpy(fit2_params, fit1_params, sizeof(fit1_params));
JTJ = (JTJ + JTJ.transpose()) * 0.5f; // fix numerical issues
matrix::SquareMatrix<float, 4> JTJ2 = JTJ;
for (uint8_t i = 0; i < 4; i++) {
JTJ(i, i) += result.gradient_damping;
JTJ2(i, i) += result.gradient_damping / lma_damping;
}
if (!JTJ.I(JTJ)) {
result.result = iteration_result::STATUS::FAILURE;
return;
}
if (!JTJ2.I(JTJ2)) {
result.result = iteration_result::STATUS::FAILURE;
return;
}
for (uint8_t row = 0; row < 4; row++) {
for (uint8_t col = 0; col < 4; col++) {
fit1_params[row] -= JTFI[col] * JTJ(row, col);
fit2_params[row] -= JTFI[col] * JTJ2(row, col);
}
}
// Calculate mean squared residuals
for (uint16_t k = 0; k < samples_collected; k++) {
float A = (params.diag(0) * (x[k] - fit1_params[1])) + (params.offdiag(0) * (y[k] - fit1_params[2])) +
(params.offdiag(1) *
(z[k] + fit1_params[3]));
float B = (params.offdiag(0) * (x[k] - fit1_params[1])) + (params.diag(1) * (y[k] - fit1_params[2])) +
(params.offdiag(2) *
(z[k] + fit1_params[3]));
float C = (params.offdiag(1) * (x[k] - fit1_params[1])) + (params.offdiag(2) * (y[k] - fit1_params[2])) + (params.diag(
2) *
(z[k] - fit1_params[3]));
float length = sqrtf(A * A + B * B + C * C);
residual = fit1_params[0] - length;
fit1 += residual * residual;
A = (params.diag(0) * (x[k] - fit2_params[1])) + (params.offdiag(0) * (y[k] - fit2_params[2])) + (params.offdiag(1) *
(z[k] - fit2_params[3]));
B = (params.offdiag(0) * (x[k] - fit2_params[1])) + (params.diag(1) * (y[k] - fit2_params[2])) + (params.offdiag(2) *
(z[k] - fit2_params[3]));
C = (params.offdiag(1) * (x[k] - fit2_params[1])) + (params.offdiag(2) * (y[k] - fit2_params[2])) + (params.diag(2) *
(z[k] - fit2_params[3]));
length = sqrtf(A * A + B * B + C * C);
residual = fit2_params[0] - length;
fit2 += residual * residual;
}
fit1 = sqrtf(fit1) / samples_collected;
fit2 = sqrtf(fit2) / samples_collected;
if (fit1 > result.cost && fit2 > result.cost) {
result.gradient_damping *= lma_damping;
} else if (fit2 < result.cost && fit2 < fit1) {
result.gradient_damping /= lma_damping;
memcpy(fit1_params, fit2_params, sizeof(fit1_params));
fitness = fit2;
} else if (fit1 < result.cost) {
fitness = fit1;
}
//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
if (PX4_ISFINITE(fitness) && fitness <= result.cost) {
result.cost = fitness;
params.radius = fit1_params[0];
params.offset(0) = fit1_params[1];
params.offset(1) = fit1_params[2];
params.offset(2) = fit1_params[3];
result.result = iteration_result::STATUS::SUCCESS;
} else {
result.result = iteration_result::STATUS::FAILURE;
}
}
void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[],
unsigned int samples_collected, sphere_params &params, iteration_result &result)
{
// Run Sphere Fit using Levenberg Marquardt LSq Fit
const float lma_damping = 10.0f;
float fitness = result.cost;
float fit1 = 0.0f;
float fit2 = 0.0f;
matrix::SquareMatrix<float, 9> JTJ;
float JTFI[9] {};
float residual = 0.0f;
float ellipsoid_jacob[9];
// Gauss Newton Part common for all kind of extensions including LM
for (uint16_t k = 0; k < samples_collected; k++) {
// Calculate Jacobian
float A = (params.diag(0) * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) +
(params.offdiag(1) * (z[k] - params.offset(2)));
float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1) * (y[k] - params.offset(1))) +
(params.offdiag(2) * (z[k] - params.offset(2)));
float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) +
(params.diag(2) * (z[k] - params.offset(2)));
float length = sqrtf(A * A + B * B + C * C);
residual = params.radius - length;
fit1 += residual * residual;
// 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
ellipsoid_jacob[0] = 1.0f * (((params.diag(0) * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length);
ellipsoid_jacob[1] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1) * B) + (params.offdiag(2) * C)) / length);
ellipsoid_jacob[2] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2) * C)) / length);
// 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
ellipsoid_jacob[3] = -1.0f * ((x[k] - params.offset(0)) * A) / length;
ellipsoid_jacob[4] = -1.0f * ((y[k] - params.offset(1)) * B) / length;
ellipsoid_jacob[5] = -1.0f * ((z[k] - params.offset(2)) * C) / length;
// 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
ellipsoid_jacob[6] = -1.0f * (((y[k] - params.offset(1)) * A) + ((x[k] - params.offset(0)) * B)) / length;
ellipsoid_jacob[7] = -1.0f * (((z[k] - params.offset(2)) * A) + ((x[k] - params.offset(0)) * C)) / length;
ellipsoid_jacob[8] = -1.0f * (((z[k] - params.offset(2)) * B) + ((y[k] - params.offset(1)) * C)) / length;
for (uint8_t i = 0; i < 9; i++) {
// compute JTJ
for (uint8_t j = 0; j < 9; j++) {
JTJ(i, j) += ellipsoid_jacob[i] * ellipsoid_jacob[j];
}
JTFI[i] += ellipsoid_jacob[i] * residual;
}
}
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
float fit1_params[9] = {params.offset(0), params.offset(1), params.offset(2), params.diag(0), params.diag(1), params.diag(2), params.offdiag(0), params.offdiag(1), params.offdiag(2)};
float fit2_params[9];
memcpy(fit2_params, fit1_params, sizeof(fit1_params));
matrix::SquareMatrix<float, 9> JTJ2 = JTJ;
for (uint8_t i = 0; i < 9; i++) {
JTJ(i, i) += result.gradient_damping;
JTJ2(i, i) += result.gradient_damping / lma_damping;
}
if (!JTJ.I(JTJ)) {
result.result = iteration_result::STATUS::FAILURE;
return;
}
if (!JTJ2.I(JTJ2)) {
result.result = iteration_result::STATUS::FAILURE;
return;
}
for (uint8_t row = 0; row < 9; row++) {
for (uint8_t col = 0; col < 9; col++) {
fit1_params[row] -= JTFI[col] * JTJ(row, col);
fit2_params[row] -= JTFI[col] * JTJ2(row, col);
}
}
// Calculate mean squared residuals
for (uint16_t k = 0; k < samples_collected; k++) {
float A = (fit1_params[3] * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] *
(z[k] - fit1_params[2]));
float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4] * (y[k] - fit1_params[1])) + (fit1_params[8] *
(z[k] - fit1_params[2]));
float C = (fit1_params[7] * (x[k] - fit1_params[0])) + (fit1_params[8] * (y[k] - fit1_params[1])) + (fit1_params[5] *
(z[k] - fit1_params[2]));
float length = sqrtf(A * A + B * B + C * C);
residual = params.radius - length;
fit1 += residual * residual;
A = (fit2_params[3] * (x[k] - fit2_params[0])) + (fit2_params[6] * (y[k] - fit2_params[1])) + (fit2_params[7] *
(z[k] - fit2_params[2]));
B = (fit2_params[6] * (x[k] - fit2_params[0])) + (fit2_params[4] * (y[k] - fit2_params[1])) + (fit2_params[8] *
(z[k] - fit2_params[2]));
C = (fit2_params[7] * (x[k] - fit2_params[0])) + (fit2_params[8] * (y[k] - fit2_params[1])) + (fit2_params[5] *
(z[k] - fit2_params[2]));
length = sqrtf(A * A + B * B + C * C);
residual = params.radius - length;
fit2 += residual * residual;
}
fit1 = sqrtf(fit1) / samples_collected;
fit2 = sqrtf(fit2) / samples_collected;
if (fit1 > result.cost && fit2 > result.cost) {
result.gradient_damping *= lma_damping;
} else if (fit2 < result.cost && fit2 < fit1) {
result.gradient_damping /= lma_damping;
memcpy(fit1_params, fit2_params, sizeof(fit1_params));
fitness = fit2;
} else if (fit1 < result.cost) {
fitness = fit1;
}
//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
if (PX4_ISFINITE(fitness) && fitness <= result.cost) {
result.cost = fitness;
params.offset(0) = fit1_params[0];
params.offset(1) = fit1_params[1];
params.offset(2) = fit1_params[2];
params.diag(0) = fit1_params[3];
params.diag(1) = fit1_params[4];
params.diag(2) = fit1_params[5];
params.offdiag(0) = fit1_params[6];
params.offdiag(1) = fit1_params[7];
params.offdiag(2) = fit1_params[8];
result.result = iteration_result::STATUS::SUCCESS;
} else {
result.result = iteration_result::STATUS::FAILURE;
}
}
int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params &params,
bool full_ellipsoid)
{
const int max_iterations = 100;
const int min_iterations = 10;
const float cost_threshold = 0.01;
const float step_threshold = 0.001;
const float min_radius = 0.2;
const float max_radius = 0.7;
iteration_result iter;
iter.cost = 1e30f;
iter.gradient_damping = 1;
bool success = false;
for (int i = 0; i < max_iterations; i++) {
if (full_ellipsoid) {
lm_ellipsoid_fit_iteration(x, y, z, samples_collected, params, iter);
} else {
lm_sphere_fit_iteration(x, y, z, samples_collected, params, iter);
}
if (iter.result == iteration_result::STATUS::SUCCESS
&& min_radius < params.radius && params.radius < max_radius
&& i > min_iterations && (iter.cost < cost_threshold || iter.gradient_damping < step_threshold)) {
success = true;
break;
}
}
if (success) {
return PX4_OK;
}
return 1;
}

63
src/prometheus_px4/src/modules/commander/lm_fit.hpp
Unescape View File

@ -1,63 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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.
*
****************************************************************************/
#pragma once
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/defines.h>
struct sphere_params {
matrix::Vector3f offset, diag{1.f, 1.f, 1.f}, offdiag;
float radius{0.2f};
};
/**
* Least-squares fit of a sphere to a set of points.
*
* Fits a sphere to a set of points on the sphere surface.
*
* @param x point coordinates on the X axis
* @param y point coordinates on the Y axis
* @param z point coordinates on the Z axis
* @param samples_collected number of points
* @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
* @param params the values to be optimized
* @param full_ellipsoid whether to just optimize a sphere, or do an ellipsoid optimization
*
* NB!! If you optimize the full ellipsoid, you must have already optimized without the full ellipsoid
*
* @return 0 on success, 1 on failure
*/
int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params &params,
bool full_ellipsoid);

1043
src/prometheus_px4/src/modules/commander/mag_calibration.cpp
View File

File diff suppressed because it is too large Load Diff

50
src/prometheus_px4/src/modules/commander/mag_calibration.h
Unescape View File

@ -1,50 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_calibration.h
* Magnetometer calibration routine
*/
#ifndef MAG_CALIBRATION_H_
#define MAG_CALIBRATION_H_
#include <math.h>
#include <stdint.h>
#include <uORB/uORB.h>
int do_mag_calibration(orb_advert_t *mavlink_log_pub);
int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians = 0, float latitude = NAN,
float longitude = NAN);
#endif /* MAG_CALIBRATION_H_ */

238
src/prometheus_px4/src/modules/commander/mag_calibration_test.cpp
Unescape View File

@ -1,238 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2021 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.
*
****************************************************************************/
/**
* Test code for the Magnetometer calibration routine
* Run this test only using make tests TESTFILTER=mag_calibration
*
* @author Mathieu Bresciani <mathieu@auterion.com>
*/
#include <gtest/gtest.h>
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/defines.h>
#include "lm_fit.hpp"
#include "mag_calibration_test_data.h"
using matrix::Vector3f;
class MagCalTest : public ::testing::Test
{
public:
void generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str);
/* Generate regularly spaced data on a sphere
* Ref.: How to generate equidistributed points on the surface of a sphere, Markus Deserno, 2004
*/
void generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str);
void modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets, Vector3f scale_factors);
};
void MagCalTest::generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str)
{
float psi = 0.f;
float theta = 0.f;
const float d_angle = 2.f * M_PI_F / float(n_samples / 2);
for (int i = 0; i < int(n_samples / 2); i++) {
x[i] = mag_str * sinf(psi);
y[i] = mag_str * cosf(psi);
z[i] = 0.f;
psi += d_angle;
}
for (int i = int(n_samples / 2); i < int(n_samples); i++) {
x[i] = mag_str * sinf(theta);
y[i] = 0.f;
z[i] = mag_str * cosf(theta);
theta += d_angle;
}
}
void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str)
{
const float a = 4.f * M_PI_F * mag_str * mag_str / n_samples;
const float d = sqrtf(a);
const int m_theta = static_cast<int>(M_PI_F / d);
const float d_theta = M_PI_F / static_cast<float>(m_theta);
const float d_phi = a / d_theta;
unsigned int n_count = 0;
for (int m = 0; m < m_theta; m++) {
const float theta = M_PI_F * (m + 0.5f) / static_cast<float>(m_theta);
const int m_phi = static_cast<int>(2.f * M_PI_F * sinf(theta / d_phi));
for (int n = 0; n < m_phi; n++) {
const float phi = 2.f * M_PI_F * n / static_cast<float>(m_phi);
x[n_count] = mag_str * sinf(theta) * cosf(phi);
y[n_count] = mag_str * sinf(theta) * sinf(phi);
z[n_count] = mag_str * cosf(theta);
n_count++;
}
}
if (n_count > n_samples) {
printf("Error placing samples, n = %d\n", n_count);
return;
}
// Padd with constant data
while (n_count < n_samples) {
x[n_count] = x[n_count - 1];
y[n_count] = y[n_count - 1];
z[n_count] = z[n_count - 1];
n_count++;
}
}
void MagCalTest::modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets,
Vector3f scale_factors)
{
for (unsigned int k = 0; k < n_samples; k++) {
x[k] = x[k] * scale_factors(0) + offsets(0);
y[k] = y[k] * scale_factors(1) + offsets(1);
z[k] = z[k] * scale_factors(2) + offsets(2);
}
}
TEST_F(MagCalTest, sphere2Sides)
{
// GIVEN: a dataset of points located on two orthogonal circles
// perfectly centered on the origin
static constexpr unsigned int N_SAMPLES = 240;
const float mag_str_true = 0.4f;
const Vector3f offset_true;
const Vector3f scale_true = {1.f, 1.f, 1.f};
float x[N_SAMPLES];
float y[N_SAMPLES];
float z[N_SAMPLES];
generate2SidesMagData(x, y, z, N_SAMPLES, mag_str_true);
// WHEN: fitting a sphere with the data and given a wrong initial radius
sphere_params sphere;
sphere.diag = {1.f, 1.f, 1.f};
sphere.radius = 0.2;
int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false);
// THEN: the algorithm should converge in a single step
EXPECT_EQ(success, PX4_OK);
EXPECT_NEAR(sphere.radius, mag_str_true, 0.001f) << "radius: " << sphere.radius;
EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.001f) << "offset X: " << sphere.offset(0);
EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.001f) << "offset Y: " << sphere.offset(1);
EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.001f) << "offset Z: " << sphere.offset(2);
EXPECT_NEAR(sphere.diag(0), scale_true(0), 0.001f) << "scale X: " << sphere.diag(0);
EXPECT_NEAR(sphere.diag(1), scale_true(1), 0.001f) << "scale Y: " << sphere.diag(1);
EXPECT_NEAR(sphere.diag(2), scale_true(2), 0.001f) << "scale Z: " << sphere.diag(2);
}
TEST_F(MagCalTest, sphereRegularlySpaced)
{
// GIVEN: a dataset of regularly spaced points
// on a perfect sphere but not centered on the origin
static constexpr unsigned int N_SAMPLES = 240;
const float mag_str_true = 0.4f;
const Vector3f offset_true = {-1.07f, 0.35f, -0.78f};
const Vector3f scale_true = {1.f, 1.f, 1.f};
float x[N_SAMPLES];
float y[N_SAMPLES];
float z[N_SAMPLES];
generateRegularData(x, y, z, N_SAMPLES, mag_str_true);
modifyOffsetScale(x, y, z, N_SAMPLES, offset_true, scale_true);
// WHEN: fitting a sphere to the data
sphere_params sphere;
sphere.diag = {1.f, 1.f, 1.f};
sphere.radius = 0.2;
int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false);
// THEN: the algorithm should converge in a few iterations and
// find the correct parameters
EXPECT_EQ(success, PX4_OK);
EXPECT_NEAR(sphere.radius, mag_str_true, 0.001f) << "radius: " << sphere.radius;
EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.001f) << "offset X: " << sphere.offset(0);
EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.001f) << "offset Y: " << sphere.offset(1);
EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.001f) << "offset Z: " << sphere.offset(2);
EXPECT_NEAR(sphere.diag(0), scale_true(0), 0.001f) << "scale X: " << scale_true(0);
EXPECT_NEAR(sphere.diag(1), scale_true(1), 0.001f) << "scale Y: " << scale_true(1);
EXPECT_NEAR(sphere.diag(2), scale_true(2), 0.001f) << "scale Z: " << scale_true(2);
}
TEST_F(MagCalTest, replayTestData)
{
// GIVEN: a real test dataset with large offsets
// and where the two first iterations of the LM algorithm
// produces a negative radius and a constant fitness value
constexpr unsigned int N_SAMPLES = 231;
const float mag_str_true = 0.4f;
const Vector3f offset_true = {-0.18f, 0.05f, -0.58f};
// WHEN: fitting a sphere to the data
sphere_params sphere;
sphere.diag = {1.f, 1.f, 1.f};
sphere.radius = 0.2;
int sphere_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, sphere, false);
// THEN: the algorithm should converge and find the correct parameters
EXPECT_EQ(sphere_success, PX4_OK);
EXPECT_NEAR(sphere.radius, mag_str_true, 0.1f) << "radius: " << sphere.radius;
EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.01f) << "offset X: " << sphere.offset(0);
EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.01f) << "offset Y: " << sphere.offset(1);
EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.01f) << "offset Z: " << sphere.offset(2);
printf("Ellipsoid fit\n");
sphere_params ellipsoid;
ellipsoid.diag = {1.f, 1.f, 1.f};
ellipsoid.radius = 0.2;
int ellipsoid_step_1_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, false);
int ellipsoid_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, true);
const Vector3f scale_true = {1.f, 1.06f, 0.94f};
EXPECT_EQ(ellipsoid_step_1_success, PX4_OK);
EXPECT_EQ(ellipsoid_success, PX4_OK);
EXPECT_NEAR(ellipsoid.radius, mag_str_true, 0.1f) << "radius: " << sphere.radius;
EXPECT_NEAR(ellipsoid.offset(0), offset_true(0), 0.01f) << "offset X: " << ellipsoid.offset(0);
EXPECT_NEAR(ellipsoid.offset(1), offset_true(1), 0.01f) << "offset Y: " << ellipsoid.offset(1);
EXPECT_NEAR(ellipsoid.offset(2), offset_true(2), 0.01f) << "offset Z: " << ellipsoid.offset(2);
EXPECT_NEAR(ellipsoid.diag(0), scale_true(0), 0.01f) << "scale X: " << ellipsoid.diag(0);
EXPECT_NEAR(ellipsoid.diag(1), scale_true(1), 0.01f) << "scale Y: " << ellipsoid.diag(1);
EXPECT_NEAR(ellipsoid.diag(2), scale_true(2), 0.01f) << "scale Z: " << ellipsoid.diag(2);
}

42
src/prometheus_px4/src/modules/commander/mag_calibration_test_data.h
View File

File diff suppressed because one or more lines are too long

88
src/prometheus_px4/src/modules/commander/px4_custom_mode.h
Unescape View File

@ -1,88 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 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 px4_custom_mode.h
* PX4 custom flight modes
*
*/
#ifndef PX4_CUSTOM_MODE_H_
#define PX4_CUSTOM_MODE_H_
#include <stdint.h>
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_ALTCTL,
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
PX4_CUSTOM_MAIN_MODE_STABILIZED,
PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
};
enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
PX4_CUSTOM_SUB_MODE_AUTO_RTL,
PX4_CUSTOM_SUB_MODE_AUTO_LAND,
PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND
};
enum PX4_CUSTOM_SUB_MODE_POSCTL {
PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL = 0,
PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT
};
union px4_custom_mode {
struct {
uint16_t reserved;
uint8_t main_mode;
uint8_t sub_mode;
};
uint32_t data;
float data_float;
struct {
uint16_t reserved_hl;
uint16_t custom_mode_hl;
};
};
#endif /* PX4_CUSTOM_MODE_H_ */

104
src/prometheus_px4/src/modules/commander/rc_calibration.cpp
Unescape View File

@ -1,104 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 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 rc_calibration.cpp
* Remote Control calibration routine
*/
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <px4_platform_common/defines.h>
#include "rc_calibration.h"
#include "commander_helper.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
{
uORB::Subscription manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
px4_usleep(400000);
manual_control_setpoint_s manual_control_setpoint{};
bool changed = manual_control_setpoint_sub.updated();
if (!changed) {
mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
return PX4_ERROR;
}
manual_control_setpoint_sub.copy(&manual_control_setpoint);
/* load trim values which are active */
float roll_trim_active;
param_get(param_find("TRIM_ROLL"), &roll_trim_active);
float pitch_trim_active;
param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
float yaw_trim_active;
param_get(param_find("TRIM_YAW"), &yaw_trim_active);
/* get manual control scale values */
float roll_scale;
param_get(param_find("FW_MAN_R_SC"), &roll_scale);
float pitch_scale;
param_get(param_find("FW_MAN_P_SC"), &pitch_scale);
float yaw_scale;
param_get(param_find("FW_MAN_Y_SC"), &yaw_scale);
/* set parameters: the new trim values are the combination of active trim values
and the values coming from the remote control of the user
*/
float p = manual_control_setpoint.y * roll_scale + roll_trim_active;
int p1r = param_set(param_find("TRIM_ROLL"), &p);
/*
we explicitly swap sign here because the trim is added to the actuator controls
which are moving in an inverse sense to manual pitch inputs
*/
p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active;
int p2r = param_set(param_find("TRIM_PITCH"), &p);
p = manual_control_setpoint.r * yaw_scale + yaw_trim_active;
int p3r = param_set(param_find("TRIM_YAW"), &p);
if (p1r != 0 || p2r != 0 || p3r != 0) {
mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
return PX4_ERROR;
}
mavlink_log_info(mavlink_log_pub, "trim cal done");
return PX4_OK;
}

47
src/prometheus_px4/src/modules/commander/rc_calibration.h
Unescape View File

@ -1,47 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 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 rc_calibration.h
* Remote Control calibration routine
*/
#ifndef RC_CALIBRATION_H_
#define RC_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_trim_calibration(orb_advert_t *mavlink_log_pub);
#endif /* RC_CALIBRATION_H_ */

1173
src/prometheus_px4/src/modules/commander/state_machine_helper.cpp
View File

File diff suppressed because it is too large Load Diff

164
src/prometheus_px4/src/modules/commander/state_machine_helper.h
Unescape View File

@ -1,164 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file state_machine_helper.h
* State machine helper functions definitions
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef STATE_MACHINE_HELPER_H_
#define STATE_MACHINE_HELPER_H_
#include <drivers/drv_hrt.h>
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/commander_state.h>
#include <uORB/topics/vehicle_status_flags.h>
typedef enum {
TRANSITION_DENIED = -1,
TRANSITION_NOT_CHANGED = 0,
TRANSITION_CHANGED
} transition_result_t;
enum class link_loss_actions_t {
DISABLED = 0,
AUTO_LOITER = 1, // Hold mode
AUTO_RTL = 2, // Return mode
AUTO_LAND = 3, // Land mode
TERMINATE = 5, // Terminate flight (set actuator outputs to failsafe values, and stop controllers)
LOCKDOWN = 6, // Lock actuators (set actuator outputs to disarmed values)
};
enum class offboard_loss_actions_t {
DISABLED = -1,
AUTO_LAND = 0, // Land mode
AUTO_LOITER = 1, // Hold mode
AUTO_RTL = 2, // Return mode
TERMINATE = 3, // Terminate flight (set actuator outputs to failsafe values, and stop controllers)
LOCKDOWN = 4, // Lock actuators (set actuator outputs to disarmed values)
};
enum class offboard_loss_rc_actions_t {
DISABLED = -1, // Disabled
MANUAL_POSITION = 0, // Position mode
MANUAL_ALTITUDE = 1, // Altitude mode
MANUAL_ATTITUDE = 2, // Manual
AUTO_RTL = 3, // Return mode
AUTO_LAND = 4, // Land mode
AUTO_LOITER = 5, // Hold mode
TERMINATE = 6, // Terminate flight (set actuator outputs to failsafe values, and stop controllers)
LOCKDOWN = 7, // Lock actuators (set actuator outputs to disarmed values)
};
enum class position_nav_loss_actions_t {
ALTITUDE_MANUAL = 0, // Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
};
extern const char *const arming_state_names[];
extern const char *const nav_state_names[];
enum class arm_disarm_reason_t {
TRANSITION_TO_STANDBY = 0,
RC_STICK = 1,
RC_SWITCH = 2,
COMMAND_INTERNAL = 3,
COMMAND_EXTERNAL = 4,
MISSION_START = 5,
SAFETY_BUTTON = 6,
AUTO_DISARM_LAND = 7,
AUTO_DISARM_PREFLIGHT = 8,
KILL_SWITCH = 9,
LOCKDOWN = 10,
FAILURE_DETECTOR = 11,
SHUTDOWN = 12,
UNIT_TEST = 13
};
enum RCLossExceptionBits {
RCL_EXCEPT_MISSION = (1 << 0),
RCL_EXCEPT_HOLD = (1 << 1),
RCL_EXCEPT_OFFBOARD = (1 << 2)
};
transition_result_t
arming_state_transition(vehicle_status_s &status, const safety_s &safety, const arming_state_t new_arming_state,
actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
transition_result_t
main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state);
void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason);
bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_state_s &internal_state,
orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
const offboard_loss_rc_actions_t offb_loss_rc_act,
const position_nav_loss_actions_t posctl_nav_loss_act,
const float param_com_rcl_act_t, const int param_com_rcl_except);
/*
* Checks the validty of position data against the requirements of the current navigation
* mode and switches mode if position data required is not available.
*/
bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
// COM_LOW_BAT_ACT parameter values
typedef enum LOW_BAT_ACTION {
WARNING = 0, // Warning
RETURN = 1, // Return mode
LAND = 2, // Land mode
RETURN_OR_LAND = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
} low_battery_action_t;
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, const uint8_t battery_warning,
const low_battery_action_t low_bat_action);
#endif /* STATE_MACHINE_HELPER_H_ */

174
src/prometheus_px4/src/modules/commander/worker_thread.cpp
Unescape View File

@ -1,174 +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.
*
****************************************************************************/
#include "worker_thread.hpp"
#include "accelerometer_calibration.h"
#include "airspeed_calibration.h"
#include "calibration_routines.h"
#include "esc_calibration.h"
#include "gyro_calibration.h"
#include "level_calibration.h"
#include "mag_calibration.h"
#include "rc_calibration.h"
#include <px4_platform_common/log.h>
#include <parameters/param.h>
WorkerThread::~WorkerThread()
{
if (_state.load() == (int)State::Running) {
/* wait for thread to complete */
int ret = pthread_join(_thread_handle, nullptr);
if (ret) {
PX4_ERR("join failed: %d", ret);
}
}
}
void WorkerThread::startTask(Request request)
{
if (isBusy()) {
return;
}
_request = request;
/* initialize low priority thread */
pthread_attr_t low_prio_attr;
pthread_attr_init(&low_prio_attr);
pthread_attr_setstacksize(&low_prio_attr, PX4_STACK_ADJUSTED(4804));
#ifndef __PX4_QURT
// This is not supported by QURT (yet).
struct sched_param param;
pthread_attr_getschedparam(&low_prio_attr, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
pthread_attr_setschedparam(&low_prio_attr, &param);
#endif
int ret = pthread_create(&_thread_handle, &low_prio_attr, &threadEntryTrampoline, this);
pthread_attr_destroy(&low_prio_attr);
if (ret == 0) {
_state.store((int)State::Running);
} else {
PX4_ERR("Failed to start thread (%i)", ret);
_state.store((int)State::Finished);
_ret_value = ret;
}
}
void *WorkerThread::threadEntryTrampoline(void *arg)
{
WorkerThread *worker_thread = (WorkerThread *)arg;
worker_thread->threadEntry();
return nullptr;
}
void WorkerThread::threadEntry()
{
px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid());
switch (_request) {
case Request::GyroCalibration:
_ret_value = do_gyro_calibration(&_mavlink_log_pub);
break;
case Request::MagCalibration:
_ret_value = do_mag_calibration(&_mavlink_log_pub);
break;
case Request::RCTrimCalibration:
_ret_value = do_trim_calibration(&_mavlink_log_pub);
break;
case Request::AccelCalibration:
_ret_value = do_accel_calibration(&_mavlink_log_pub);
break;
case Request::LevelCalibration:
_ret_value = do_level_calibration(&_mavlink_log_pub);
break;
case Request::AccelCalibrationQuick:
_ret_value = do_accel_calibration_quick(&_mavlink_log_pub);
break;
case Request::AirspeedCalibration:
_ret_value = do_airspeed_calibration(&_mavlink_log_pub);
break;
case Request::ESCCalibration:
_ret_value = do_esc_calibration(&_mavlink_log_pub);
break;
case Request::MagCalibrationQuick:
_ret_value = do_mag_calibration_quick(&_mavlink_log_pub, _heading_radians, _latitude, _longitude);
break;
case Request::ParamLoadDefault:
_ret_value = param_load_default();
if (_ret_value != 0) {
mavlink_log_critical(&_mavlink_log_pub, "Error loading settings");
}
break;
case Request::ParamSaveDefault:
_ret_value = param_save_default();
if (_ret_value != 0) {
mavlink_log_critical(&_mavlink_log_pub, "Error saving settings");
}
break;
case Request::ParamResetAll:
param_reset_all();
_ret_value = 0;
break;
}
_state.store((int)State::Finished); // set this last to signal the main thread we're done
}
void WorkerThread::setMagQuickData(float heading_rad, float lat, float lon)
{
_heading_radians = heading_rad;
_latitude = lat;
_longitude = lon;
}

100
src/prometheus_px4/src/modules/commander/worker_thread.hpp
Unescape View File

@ -1,100 +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.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/posix.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
/**
* @class WorkerThread
* low priority background thread, started on demand, used for:
* - calibration
* - param saving
*/
class WorkerThread
{
public:
enum class Request {
GyroCalibration,
MagCalibration,
RCTrimCalibration,
AccelCalibration,
LevelCalibration,
AccelCalibrationQuick,
AirspeedCalibration,
ESCCalibration,
MagCalibrationQuick,
ParamLoadDefault,
ParamSaveDefault,
ParamResetAll,
};
WorkerThread() = default;
~WorkerThread();
void setMagQuickData(float heading_rad, float lat, float lon);
void startTask(Request request);
bool isBusy() const { return _state.load() != (int)State::Idle; }
bool hasResult() const { return _state.load() == (int)State::Finished; }
int getResultAndReset() { _state.store((int)State::Idle); return _ret_value; }
private:
enum class State {
Idle,
Running,
Finished
};
static void *threadEntryTrampoline(void *arg);
void threadEntry();
px4::atomic_int _state{(int)State::Idle};
pthread_t _thread_handle{};
int _ret_value{};
Request _request;
orb_advert_t _mavlink_log_pub{nullptr};
// extra arguments
float _heading_radians;
float _latitude;
float _longitude;
};

110
src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
Unescape View File

@ -1,110 +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 ActuatorEffectiveness.hpp
*
* Interface for Actuator Effectiveness
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include <ControlAllocation/ControlAllocation.hpp>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_actuator_setpoint.h>
class ActuatorEffectiveness
{
public:
ActuatorEffectiveness() = default;
virtual ~ActuatorEffectiveness() = default;
static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
enum class FlightPhase {
HOVER_FLIGHT = 0,
FORWARD_FLIGHT = 1,
TRANSITION_HF_TO_FF = 2,
TRANSITION_FF_TO_HF = 3
};
/**
* Set the current flight phase
*
* @param Flight phase
*/
virtual void setFlightPhase(const FlightPhase &flight_phase)
{
_flight_phase = flight_phase;
}
/**
* Get the control effectiveness matrix if updated
*
* @return true if updated and matrix is set
*/
virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) = 0;
/**
* Get the actuator trims
*
* @return Actuator trims
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
{
return _trim;
}
/**
* Get the current flight phase
*
* @return Flight phase
*/
const FlightPhase &getFlightPhase() const
{
return _flight_phase;
}
/**
* Get the number of actuators
*/
virtual int numActuators() const = 0;
protected:
matrix::Vector<float, NUM_ACTUATORS> _trim; ///< Actuator trim
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase
};

200
src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp
Unescape View File

@ -1,200 +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 ActuatorEffectivenessMultirotor.hpp
*
* Actuator effectiveness computed from rotors position and orientation
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "ActuatorEffectivenessMultirotor.hpp"
ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor():
ModuleParams(nullptr)
{
}
bool
ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix)
{
// Check if parameters have changed
if (_updated || _parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
_updated = false;
// Get multirotor geometry
MultirotorGeometry geometry = {};
geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
_num_actuators = computeEffectivenessMatrix(geometry, matrix);
return true;
}
return false;
}
int
ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry,
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness)
{
int num_actuators = 0;
effectiveness.setZero();
for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
// Get rotor axis
matrix::Vector3f axis(
geometry.rotors[i].axis_x,
geometry.rotors[i].axis_y,
geometry.rotors[i].axis_z
);
// Normalize axis
float axis_norm = axis.norm();
if (axis_norm > FLT_EPSILON) {
axis /= axis_norm;
} else {
// Bad axis definition, ignore this rotor
continue;
}
// Get rotor position
matrix::Vector3f position(
geometry.rotors[i].position_x,
geometry.rotors[i].position_y,
geometry.rotors[i].position_z
);
// Get coefficients
float ct = geometry.rotors[i].thrust_coef;
float km = geometry.rotors[i].moment_ratio;
if (fabsf(ct) < FLT_EPSILON) {
continue;
}
// Compute thrust generated by this rotor
matrix::Vector3f thrust = ct * axis;
// Compute moment generated by this rotor
matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
// Fill corresponding items in effectiveness matrix
for (size_t j = 0; j < 3; j++) {
effectiveness(j, i) = moment(j);
effectiveness(j + 3, i) = thrust(j);
}
num_actuators = i + 1;
}
return num_actuators;
}

Some files were not shown because too many files have changed in this diff Show More

Write Preview
Loading…
Cancel
Save