diff --git a/src/prometheus_px4/src/modules/airship_att_control/CMakeLists.txt b/src/prometheus_px4/src/modules/airship_att_control/CMakeLists.txt new file mode 100644 index 0000000..9771e2a --- /dev/null +++ b/src/prometheus_px4/src/modules/airship_att_control/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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 + ) diff --git a/src/prometheus_px4/src/modules/airship_att_control/airship_att_control.hpp b/src/prometheus_px4/src/modules/airship_att_control/airship_att_control.hpp new file mode 100644 index 0000000..201d791 --- /dev/null +++ b/src/prometheus_px4/src/modules/airship_att_control/airship_att_control.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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, 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 _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 */ + +}; diff --git a/src/prometheus_px4/src/modules/airship_att_control/airship_att_control_main.cpp b/src/prometheus_px4/src/modules/airship_att_control/airship_att_control_main.cpp new file mode 100644 index 0000000..db4f813 --- /dev/null +++ b/src/prometheus_px4/src/modules/airship_att_control/airship_att_control_main.cpp @@ -0,0 +1,213 @@ +/**************************************************************************** + * + * 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 + */ + +#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); +} diff --git a/src/prometheus_px4/src/modules/airspeed_selector/CMakeLists.txt b/src/prometheus_px4/src/modules/airspeed_selector/CMakeLists.txt new file mode 100644 index 0000000..6c2ad20 --- /dev/null +++ b/src/prometheus_px4/src/modules/airspeed_selector/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 +) diff --git a/src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_main.cpp new file mode 100644 index 0000000..d347734 --- /dev/null +++ b/src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -0,0 +1,675 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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, 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_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/ + uORB::PublicationMulti _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_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) _param_west_w_p_noise, + (ParamFloat) _param_west_sc_p_noise, + (ParamFloat) _param_west_tas_noise, + (ParamFloat) _param_west_beta_noise, + (ParamInt) _param_west_tas_gate, + (ParamInt) _param_west_beta_gate, + (ParamInt) _param_west_scale_estimation_on, + (ParamFloat) _param_west_airspeed_scale, + (ParamInt) _param_airspeed_primary_index, + (ParamInt) _param_airspeed_checks_on, + (ParamInt) _param_airspeed_fallback_gw, + + (ParamFloat) _tas_innov_threshold, /**< innovation check threshold */ + (ParamFloat) _tas_innov_integ_threshold, /**< innovation check integrator threshold */ + (ParamInt) _checks_fail_delay, /**< delay to declare airspeed invalid */ + (ParamInt) _checks_clear_delay, /**< delay to declare airspeed valid again */ + + (ParamFloat) _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); +} diff --git a/src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_params.c b/src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_params.c new file mode 100644 index 0000000..dac73ae --- /dev/null +++ b/src/prometheus_px4/src/modules/airspeed_selector/airspeed_selector_params.c @@ -0,0 +1,186 @@ + +/** + * 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); diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp new file mode 100644 index 0000000..7c3ffb2 --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 +#include + +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 &saturation_positive, + const matrix::Vector &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(); +} diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp new file mode 100644 index 0000000..aee955c --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * 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 + +#include + +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 &saturation_positive, + const matrix::Vector &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()}; ///< inertia matrix + + // States + matrix::Vector3f _angular_velocity_int; + matrix::Vector _saturation_positive; + matrix::Vector _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 +}; diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp new file mode 100644 index 0000000..9f4c85a --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * 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 +#include + +using namespace matrix; + +TEST(AngularVelocityControlTest, AllZeroCase) +{ + AngularVelocityControl control; + control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false); + Vector3f torque = control.getTorqueSetpoint(); + EXPECT_EQ(torque, Vector3f()); +} diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt new file mode 100644 index 0000000..05bb7dd --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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) diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.cpp new file mode 100644 index 0000000..1d8dcba --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -0,0 +1,342 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +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(¶m_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 saturation_positive; + Vector 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); +} diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.hpp b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.hpp new file mode 100644 index 0000000..c805297 --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/AngularVelocityController.hpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class AngularVelocityController : public ModuleBase, 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_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ + uORB::Publication _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */ + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */ + uORB::Publication _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) _param_avc_x_p, + (ParamFloat) _param_avc_x_i, + (ParamFloat) _param_avc_x_i_lim, + (ParamFloat) _param_avc_x_d, + (ParamFloat) _param_avc_x_ff, + (ParamFloat) _param_avc_x_k, + + (ParamFloat) _param_avc_y_p, + (ParamFloat) _param_avc_y_i, + (ParamFloat) _param_avc_y_i_lim, + (ParamFloat) _param_avc_y_d, + (ParamFloat) _param_avc_y_ff, + (ParamFloat) _param_avc_y_k, + + (ParamFloat) _param_avc_z_p, + (ParamFloat) _param_avc_z_i, + (ParamFloat) _param_avc_z_i_lim, + (ParamFloat) _param_avc_z_d, + (ParamFloat) _param_avc_z_ff, + (ParamFloat) _param_avc_z_k, + + (ParamFloat) _param_vm_mass, + (ParamFloat) _param_vm_inertia_xx, + (ParamFloat) _param_vm_inertia_yy, + (ParamFloat) _param_vm_inertia_zz, + (ParamFloat) _param_vm_inertia_xy, + (ParamFloat) _param_vm_inertia_xz, + (ParamFloat) _param_vm_inertia_yz, + + (ParamFloat) _param_mpc_thr_hover, + (ParamBool) _param_mpc_use_hte + ) + +}; diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/CMakeLists.txt b/src/prometheus_px4/src/modules/angular_velocity_controller/CMakeLists.txt new file mode 100644 index 0000000..4cfc369 --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# 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 + ) diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/angular_velocity_controller_params.c b/src/prometheus_px4/src/modules/angular_velocity_controller/angular_velocity_controller_params.c new file mode 100644 index 0000000..88c61c4 --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/angular_velocity_controller_params.c @@ -0,0 +1,297 @@ +/**************************************************************************** + * + * 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 + * @author Anton Babushkin + * @author Julien Lecoeur + */ + +/** + * 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); diff --git a/src/prometheus_px4/src/modules/angular_velocity_controller/vehicle_model_params.c b/src/prometheus_px4/src/modules/angular_velocity_controller/vehicle_model_params.c new file mode 100644 index 0000000..a141779 --- /dev/null +++ b/src/prometheus_px4/src/modules/angular_velocity_controller/vehicle_model_params.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * 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 + */ + +/** + * 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); diff --git a/src/prometheus_px4/src/modules/attitude_estimator_q/CMakeLists.txt b/src/prometheus_px4/src/modules/attitude_estimator_q/CMakeLists.txt new file mode 100644 index 0000000..6563648 --- /dev/null +++ b/src/prometheus_px4/src/modules/attitude_estimator_q/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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 + ) + diff --git a/src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp new file mode 100644 index 0000000..70b4839 --- /dev/null +++ b/src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -0,0 +1,615 @@ +/**************************************************************************** + * + * 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 + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Dcmf; +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Vector3f; +using matrix::wrap_pi; + +using namespace time_literals; + +class AttitudeEstimatorQ : public ModuleBase, 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 _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) _param_att_w_acc, + (ParamFloat) _param_att_w_mag, + (ParamFloat) _param_att_w_ext_hdg, + (ParamFloat) _param_att_w_gyro_bias, + (ParamFloat) _param_att_mag_decl, + (ParamInt) _param_att_mag_decl_a, + (ParamInt) _param_att_ext_hdg_m, + (ParamInt) _param_att_acc_comp, + (ParamFloat) _param_att_bias_mas, + (ParamInt) _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); +} diff --git a/src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_params.c new file mode 100644 index 0000000..0cea30c --- /dev/null +++ b/src/prometheus_px4/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 + */ + +/** + * 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); diff --git a/src/prometheus_px4/src/modules/battery_status/CMakeLists.txt b/src/prometheus_px4/src/modules/battery_status/CMakeLists.txt new file mode 100644 index 0000000..df7c996 --- /dev/null +++ b/src/prometheus_px4/src/modules/battery_status/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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 + ) diff --git a/src/prometheus_px4/src/modules/battery_status/analog_battery.cpp b/src/prometheus_px4/src/modules/battery_status/analog_battery.cpp new file mode 100644 index 0000000..51b4fbd --- /dev/null +++ b/src/prometheus_px4/src/modules/battery_status/analog_battery.cpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * 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 +#include +#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(_analog_param_handles.v_div_old, _analog_param_handles.v_div, &_analog_params.v_div_old, + &_analog_params.v_div, _first_parameter_update); + migrateParam(_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(_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(); +} diff --git a/src/prometheus_px4/src/modules/battery_status/analog_battery.h b/src/prometheus_px4/src/modules/battery_status/analog_battery.h new file mode 100644 index 0000000..ddfc8e7 --- /dev/null +++ b/src/prometheus_px4/src/modules/battery_status/analog_battery.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 +#include + +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; +}; diff --git a/src/prometheus_px4/src/modules/battery_status/analog_battery_params_common.c b/src/prometheus_px4/src/modules/battery_status/analog_battery_params_common.c new file mode 100644 index 0000000..ed33d09 --- /dev/null +++ b/src/prometheus_px4/src/modules/battery_status/analog_battery_params_common.c @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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); diff --git a/src/prometheus_px4/src/modules/battery_status/analog_battery_params_deprecated.c b/src/prometheus_px4/src/modules/battery_status/analog_battery_params_deprecated.c new file mode 100644 index 0000000..9be130f --- /dev/null +++ b/src/prometheus_px4/src/modules/battery_status/analog_battery_params_deprecated.c @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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); diff --git a/src/prometheus_px4/src/modules/battery_status/battery_status.cpp b/src/prometheus_px4/src/modules/battery_status/battery_status.cpp new file mode 100644 index 0000000..a509a3f --- /dev/null +++ b/src/prometheus_px4/src/modules/battery_status/battery_status.cpp @@ -0,0 +1,321 @@ +/**************************************************************************** + * + * 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 + * @author Julian Oes + * @author Thomas Gubler + * @author Anton Babushkin + * @author Beat Küng + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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, 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); +} diff --git a/src/prometheus_px4/src/modules/battery_status/module.yaml b/src/prometheus_px4/src/modules/battery_status/module.yaml new file mode 100644 index 0000000..91359c2 --- /dev/null +++ b/src/prometheus_px4/src/modules/battery_status/module.yaml @@ -0,0 +1,63 @@ +__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] \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/camera_feedback/CMakeLists.txt b/src/prometheus_px4/src/modules/camera_feedback/CMakeLists.txt new file mode 100644 index 0000000..152f2c1 --- /dev/null +++ b/src/prometheus_px4/src/modules/camera_feedback/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 + ) diff --git a/src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.cpp b/src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.cpp new file mode 100644 index 0000000..93723df --- /dev/null +++ b/src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.cpp @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * 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); +} diff --git a/src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.hpp b/src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.hpp new file mode 100644 index 0000000..bd019cb --- /dev/null +++ b/src/prometheus_px4/src/modules/camera_feedback/CameraFeedback.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class CameraFeedback : public ModuleBase, 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 _capture_pub{ORB_ID(camera_capture)}; +}; diff --git a/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp b/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp new file mode 100644 index 0000000..6541569 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp @@ -0,0 +1,298 @@ +/**************************************************************************** + * + * 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 +#include + +#include +#include +#include +#include +#include +#include +#include + +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 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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h b/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h new file mode 100644 index 0000000..74b6571 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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 + +#include +#include + +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(); diff --git a/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt b/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt new file mode 100644 index 0000000..648d5ba --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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} +) diff --git a/src/prometheus_px4/src/modules/commander/Arming/CMakeLists.txt b/src/prometheus_px4/src/modules/commander/Arming/CMakeLists.txt new file mode 100644 index 0000000..1d638ad --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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) diff --git a/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/CMakeLists.txt b/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/CMakeLists.txt new file mode 100644 index 0000000..4b8dc20 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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}) diff --git a/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp b/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp new file mode 100644 index 0000000..4a43e29 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.h b/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.h new file mode 100644 index 0000000..e1cd524 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/HealthFlags/HealthFlags.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 +#include + +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); diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt new file mode 100644 index 0000000..7923a5e --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# 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) diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp new file mode 100644 index 0000000..80abab1 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -0,0 +1,267 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp new file mode 100644 index 0000000..392f3bb --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 +#include + +#include +#include + +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); +}; diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp new file mode 100644 index 0000000..cf1eeef --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include + +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 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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp new file mode 100644 index 0000000..5c032fe --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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 +#include +#include + +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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp new file mode 100644 index 0000000..ffaceea --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include + +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_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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp new file mode 100644 index 0000000..00d48d7 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include + +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 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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp new file mode 100644 index 0000000..bf84206 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +using namespace time_literals; + +bool PreFlightCheck::cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail) +{ + bool success = true; + + uORB::SubscriptionData 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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp new file mode 100644 index 0000000..06e2f4a --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -0,0 +1,287 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include + +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_sub{ORB_ID(estimator_selector_status)}; + uORB::SubscriptionData 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_sub{ORB_ID(estimator_selector_status)}; + uORB::SubscriptionData 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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp new file mode 100644 index 0000000..8f1b5e5 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 + +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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp new file mode 100644 index 0000000..ca09bd5 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include + +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 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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp new file mode 100644 index 0000000..cad290c --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include + +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_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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp new file mode 100644 index 0000000..5c76650 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include + +// 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 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(angle_difference_limit_deg); + + if (!pass && report_status) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent", + static_cast(math::degrees(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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp new file mode 100644 index 0000000..713405e --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include + +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 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_sub{ORB_ID(estimator_status), i}; + + if (estimator_status_sub.get().mag_device_id == static_cast(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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp new file mode 100644 index 0000000..661d8fb --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 +#include +#include + +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_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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp new file mode 100644 index 0000000..66f3f69 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +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_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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp new file mode 100644 index 0000000..d0d113e --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include + +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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp new file mode 100644 index 0000000..26bee53 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * 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 +#include +#include + +/** + * 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, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_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; +} diff --git a/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp new file mode 100644 index 0000000..bfcd4d5 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 +#include + +#ifdef __PX4_DARWIN +#include +#include +#else +#include +#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"), ¶m_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; +} diff --git a/src/prometheus_px4/src/modules/commander/CMakeLists.txt b/src/prometheus_px4/src/modules/commander/CMakeLists.txt new file mode 100644 index 0000000..f9568a0 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/CMakeLists.txt @@ -0,0 +1,73 @@ +############################################################################ +# +# 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) diff --git a/src/prometheus_px4/src/modules/commander/Commander.cpp b/src/prometheus_px4/src/modules/commander/Commander.cpp new file mode 100644 index 0000000..1cf60e8 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Commander.cpp @@ -0,0 +1,4123 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 commander.cpp + * + * Main state machine / business logic + * + * @TODO This application is currently in a rewrite process. Main changes: + * - Calibration routines are moved into the event system + * - Commander is rewritten as class + * - State machines will be model driven + */ + +#include "Commander.hpp" + +/* commander module headers */ +#include "Arming/PreFlightCheck/PreFlightCheck.hpp" +#include "Arming/ArmAuthorization/ArmAuthorization.h" +#include "Arming/HealthFlags/HealthFlags.h" +#include "commander_helper.h" +#include "esc_calibration.h" +#include "px4_custom_mode.h" +#include "state_machine_helper.h" + +/* PX4 headers */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +typedef enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ + VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */ +} VEHICLE_MODE_FLAG; + +#if defined(BOARD_HAS_POWER_CONTROL) +static orb_advert_t power_button_state_pub = nullptr; +static int power_button_state_notification_cb(board_power_button_state_notification_e request) +{ + // Note: this can be called from IRQ handlers, so we publish a message that will be handled + // on the main thread of commander. + power_button_state_s button_state{}; + button_state.timestamp = hrt_absolute_time(); + const int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING; + + switch (request) { + case PWR_BUTTON_IDEL: + button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL; + break; + + case PWR_BUTTON_DOWN: + button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN; + break; + + case PWR_BUTTON_UP: + button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP; + break; + + case PWR_BUTTON_REQUEST_SHUT_DOWN: + button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN; + break; + + default: + PX4_ERR("unhandled power button state: %i", (int)request); + return ret; + } + + if (power_button_state_pub != nullptr) { + orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state); + + } else { + PX4_ERR("power_button_state_pub not properly initialized"); + } + + return ret; +} +#endif // BOARD_HAS_POWER_CONTROL + +#ifndef CONSTRAINED_FLASH +static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN, + const float param3 = NAN, const float param4 = NAN, const double param5 = static_cast(NAN), + const double param6 = static_cast(NAN), const float param7 = NAN) +{ + vehicle_command_s vcmd{}; + vcmd.command = cmd; + vcmd.param1 = param1; + vcmd.param2 = param2; + vcmd.param3 = param3; + vcmd.param4 = param4; + vcmd.param5 = param5; + vcmd.param6 = param6; + vcmd.param7 = param7; + + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + vcmd.source_system = vehicle_status_sub.get().system_id; + vcmd.target_system = vehicle_status_sub.get().system_id; + vcmd.source_component = vehicle_status_sub.get().component_id; + vcmd.target_component = vehicle_status_sub.get().component_id; + + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; + vcmd.timestamp = hrt_absolute_time(); + return vcmd_pub.publish(vcmd); +} + +static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN, + const float param3 = NAN, const float param4 = NAN, const double param5 = static_cast(NAN), + const double param6 = static_cast(NAN), const float param7 = NAN) +{ + vehicle_command_s vcmd{}; + vcmd.command = cmd; + vcmd.param1 = param1; + vcmd.param2 = param2; + vcmd.param3 = param3; + vcmd.param4 = param4; + vcmd.param5 = param5; + vcmd.param6 = param6; + vcmd.param7 = param7; + + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + vcmd.source_system = vehicle_status_sub.get().system_id; + vcmd.target_system = 0; + vcmd.source_component = vehicle_status_sub.get().component_id; + vcmd.target_component = 0; + + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; + vcmd.timestamp = hrt_absolute_time(); + return vcmd_pub.publish(vcmd); +} +#endif + +int Commander::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + print_usage("not running"); + return 1; + } + +#ifndef CONSTRAINED_FLASH + + if (!strcmp(argv[0], "calibrate")) { + if (argc > 1) { + if (!strcmp(argv[1], "gyro")) { + // gyro calibration: param1 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.0, 0.0, 0.f); + + } else if (!strcmp(argv[1], "mag")) { + if (argc > 2 && (strcmp(argv[2], "quick") == 0)) { + // magnetometer quick calibration: VEHICLE_CMD_FIXED_MAG_CAL_YAW + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW); + + } else { + // magnetometer calibration: param2 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f); + } + + } else if (!strcmp(argv[1], "accel")) { + if (argc > 2 && (strcmp(argv[2], "quick") == 0)) { + // accelerometer quick calibration: param5 = 3 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.0, 0.0, 0.f); + + } else { + // accelerometer calibration: param5 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.0, 0.0, 0.f); + } + + } else if (!strcmp(argv[1], "level")) { + // board level calibration: param5 = 2 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.0, 0.0, 0.f); + + } else if (!strcmp(argv[1], "airspeed")) { + // airspeed calibration: param6 = 2 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 2.0, 0.f); + + } else if (!strcmp(argv[1], "esc")) { + // ESC calibration: param7 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 0.0, 1.f); + + } else { + PX4_ERR("argument %s unsupported.", argv[1]); + return 1; + } + + return 0; + + } else { + PX4_ERR("missing argument"); + } + } + + if (!strcmp(argv[0], "check")) { + uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)}; + vehicle_status_s vehicle_status{}; + vehicle_status_sub.copy(&vehicle_status); + + uORB::Subscription vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + vehicle_status_flags_s vehicle_status_flags{}; + vehicle_status_flags_sub.copy(&vehicle_status_flags); + + bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, true, true, + 30_s); + PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); + + bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, safety_s{}, + PreFlightCheck::arm_requirements_t{}, vehicle_status); + PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED"); + + print_health_flags(vehicle_status); + + return 0; + } + + if (!strcmp(argv[0], "arm")) { + float param2 = 0.f; + + // 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight) + if (argc > 1 && !strcmp(argv[1], "-f")) { + param2 = 21196.f; + } + + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f, param2); + + return 0; + } + + if (!strcmp(argv[0], "disarm")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.f, 0.f); + + return 0; + } + + if (!strcmp(argv[0], "takeoff")) { + // switch to takeoff mode and arm + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f, 0.f); + + return 0; + } + + if (!strcmp(argv[0], "land")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND); + + return 0; + } + + if (!strcmp(argv[0], "transition")) { + uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)}; + vehicle_status_s vehicle_status{}; + vehicle_status_sub.copy(&vehicle_status); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, + (float)(vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ? + vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : + vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), 0.0f); + + return 0; + } + + if (!strcmp(argv[0], "mode")) { + if (argc > 1) { + + if (!strcmp(argv[1], "manual")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_MANUAL); + + } else if (!strcmp(argv[1], "altctl")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ALTCTL); + + } else if (!strcmp(argv[1], "posctl")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL); + + } else if (!strcmp(argv[1], "auto:mission")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_MISSION); + + } else if (!strcmp(argv[1], "auto:loiter")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_LOITER); + + } else if (!strcmp(argv[1], "auto:rtl")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_RTL); + + } else if (!strcmp(argv[1], "acro")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ACRO); + + } else if (!strcmp(argv[1], "offboard")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_OFFBOARD); + + } else if (!strcmp(argv[1], "stabilized")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_STABILIZED); + + } else if (!strcmp(argv[1], "auto:takeoff")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF); + + } else if (!strcmp(argv[1], "auto:land")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_LAND); + + } else if (!strcmp(argv[1], "auto:precland")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND); + + } else { + PX4_ERR("argument %s unsupported.", argv[1]); + } + + return 0; + + } else { + PX4_ERR("missing argument"); + } + } + + if (!strcmp(argv[0], "lockdown")) { + + if (argc < 2) { + Commander::print_usage("not enough arguments, missing [on, off]"); + return 1; + } + + bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION, + strcmp(argv[1], "off") ? 2.0f : 0.0f /* lockdown */, 0.0f); + + return (ret ? 0 : 1); + } + + if (!strcmp(argv[0], "pair")) { + + // GCS pairing request handled by a companion + bool ret = broadcast_vehicle_command(vehicle_command_s::VEHICLE_CMD_START_RX_PAIR, 10.f); + + return (ret ? 0 : 1); + } + + if (!strcmp(argv[0], "set_ekf_origin")) { + if (argc > 3) { + + double latitude = atof(argv[1]); + double longitude = atof(argv[2]); + float altitude = atof(argv[3]); + + // Set the ekf NED origin global coordinates. + bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN, + 0.f, 0.f, 0.0, 0.0, latitude, longitude, altitude); + return (ret ? 0 : 1); + + } else { + PX4_ERR("missing argument"); + return 0; + } + } + + +#endif + + return print_usage("unknown command"); +} + +int Commander::print_status() +{ + PX4_INFO("arming: %s", arming_state_names[_status.arming_state]); + PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]); + return 0; +} + +extern "C" __EXPORT int commander_main(int argc, char *argv[]) +{ + return Commander::main(argc, argv); +} + +bool Commander::shutdown_if_allowed() +{ + return TRANSITION_DENIED != arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN, + _armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements, + hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN); +} + +static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) +{ + switch (calling_reason) { + case arm_disarm_reason_t::TRANSITION_TO_STANDBY: return ""; + + case arm_disarm_reason_t::RC_STICK: return "RC"; + + case arm_disarm_reason_t::RC_SWITCH: return "RC (switch)"; + + case arm_disarm_reason_t::COMMAND_INTERNAL: return "internal command"; + + case arm_disarm_reason_t::COMMAND_EXTERNAL: return "external command"; + + case arm_disarm_reason_t::MISSION_START: return "mission start"; + + case arm_disarm_reason_t::SAFETY_BUTTON: return "safety button"; + + case arm_disarm_reason_t::AUTO_DISARM_LAND: return "landing"; + + case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: return "auto preflight disarming"; + + case arm_disarm_reason_t::KILL_SWITCH: return "kill-switch"; + + case arm_disarm_reason_t::LOCKDOWN: return "lockdown"; + + case arm_disarm_reason_t::FAILURE_DETECTOR: return "failure detector"; + + case arm_disarm_reason_t::SHUTDOWN: return "shutdown request"; + + case arm_disarm_reason_t::UNIT_TEST: return "unit tests"; + } + + return ""; +}; + +static constexpr const char *main_state_str(uint8_t main_state) +{ + switch (main_state) { + case commander_state_s::MAIN_STATE_MANUAL: return "Manual"; + + case commander_state_s::MAIN_STATE_ALTCTL: return "Altitude"; + + case commander_state_s::MAIN_STATE_POSCTL: return "Position"; + + case commander_state_s::MAIN_STATE_AUTO_MISSION: return "Mission"; + + case commander_state_s::MAIN_STATE_AUTO_LOITER: return "Hold"; + + case commander_state_s::MAIN_STATE_AUTO_RTL: return "RTL"; + + case commander_state_s::MAIN_STATE_ACRO: return "Acro"; + + case commander_state_s::MAIN_STATE_OFFBOARD: return "Offboard"; + + case commander_state_s::MAIN_STATE_STAB: return "Stabilized"; + + case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return "Takeoff"; + + case commander_state_s::MAIN_STATE_AUTO_LAND: return "Land"; + + case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return "Follow target"; + + case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return "Precision land"; + + case commander_state_s::MAIN_STATE_ORBIT: return "Orbit"; + + default: return "Unknown"; + } +} + +transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks) +{ + // allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming + if (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) { + run_preflight_checks = false; + } + + if (run_preflight_checks) { + if (_vehicle_control_mode.flag_control_manual_enabled) { + if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center"); + tune_negative(true); + return TRANSITION_DENIED; + + } + + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_manual_control.isThrottleLow()) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle"); + tune_negative(true); + return TRANSITION_DENIED; + } + } + + if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL) + && !_status_flags.condition_home_position_valid) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Geofence RTL requires valid home"); + tune_negative(true); + return TRANSITION_DENIED; + } + } + + transition_result_t arming_res = arming_state_transition(_status, + _safety, + vehicle_status_s::ARMING_STATE_ARMED, + _armed, + run_preflight_checks, + &_mavlink_log_pub, + _status_flags, + _arm_requirements, + hrt_elapsed_time(&_boot_timestamp), calling_reason); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(&_mavlink_log_pub, "Armed by %s", arm_disarm_reason_str(calling_reason)); + + _status_changed = true; + + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; +} + +transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason) +{ + transition_result_t arming_res = arming_state_transition(_status, + _safety, + vehicle_status_s::ARMING_STATE_STANDBY, + _armed, + false, + &_mavlink_log_pub, + _status_flags, + _arm_requirements, + hrt_elapsed_time(&_boot_timestamp), calling_reason); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s", arm_disarm_reason_str(calling_reason)); + + _status_changed = true; + + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; +} + +transition_result_t +Commander::try_mode_change(main_state_t desired_mode) +{ + transition_result_t res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + + if (res == TRANSITION_DENIED) { + + print_reject_mode(desired_mode); + + if (desired_mode == commander_state_s::MAIN_STATE_OFFBOARD) { + /* offboard does not have a fallback */ + return res; + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_RTL && (res == TRANSITION_DENIED)) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LAND && (res == TRANSITION_DENIED)) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF && (res == TRANSITION_DENIED)) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET && (res == TRANSITION_DENIED)) { + desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + + if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LOITER && (res == TRANSITION_DENIED)) { + /* fall back to position control */ + desired_mode = commander_state_s::MAIN_STATE_POSCTL; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + + if (desired_mode == commander_state_s::MAIN_STATE_POSCTL && (res == TRANSITION_DENIED)) { + /* fall back to altitude control */ + desired_mode = commander_state_s::MAIN_STATE_ALTCTL; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + + if (desired_mode == commander_state_s::MAIN_STATE_ALTCTL && (res == TRANSITION_DENIED)) { + /* fall back to stabilized */ + desired_mode = commander_state_s::MAIN_STATE_STAB; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + + if (desired_mode == commander_state_s::MAIN_STATE_STAB && (res == TRANSITION_DENIED)) { + /* fall back to manual */ + desired_mode = commander_state_s::MAIN_STATE_MANUAL; + res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); + } + } + + return res; +} + +Commander::Commander() : + ModuleParams(nullptr), + _failure_detector(this) +{ + _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); + + _land_detector.landed = true; + + // XXX for now just set sensors as initialized + _status_flags.condition_system_sensors_initialized = true; + + // We want to accept RC inputs as default + _status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; + _status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + _status.nav_state_timestamp = hrt_absolute_time(); + _status.arming_state = vehicle_status_s::ARMING_STATE_INIT; + + /* mark all signals lost as long as they haven't been found */ + _status.rc_signal_lost = true; + _status.data_link_lost = true; + + _status_flags.offboard_control_signal_lost = true; + + _status_flags.condition_power_input_valid = true; + _status_flags.rc_calibration_valid = true; + + // default for vtol is rotary wing + _vtol_status.vtol_in_rw_mode = true; + + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ + mission_init(); +} + +bool +Commander::handle_command(const vehicle_command_s &cmd) +{ + /* only handle commands that are meant to be handled by this system and component */ + if (cmd.target_system != _status.system_id || ((cmd.target_component != _status.component_id) + && (cmd.target_component != 0))) { // component_id 0: valid for all components + return false; + } + + /* result of the command */ + unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* request to set different system mode */ + switch (cmd.command) { + case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: { + + // Just switch the flight mode here, the navigator takes care of + // doing something sensible with the coordinates. Its designed + // to not require navigator and command to receive / process + // the data at the exact same time. + + // Check if a mode switch had been requested + if ((((uint32_t)cmd.param2) & 1) > 0) { + transition_result_t main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, + _status_flags, _internal_state); + + if ((main_ret != TRANSITION_DENIED)) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + mavlink_log_critical(&_mavlink_log_pub, "Reposition command rejected"); + } + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { + uint8_t base_mode = (uint8_t)cmd.param1; + uint8_t custom_main_mode = (uint8_t)cmd.param2; + uint8_t custom_sub_mode = (uint8_t)cmd.param3; + + transition_result_t main_ret = TRANSITION_NOT_CHANGED; + + if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { + /* ALTCTL */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { + /* POSCTL */ + reset_posvel_validity(); + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + if (custom_sub_mode > 0) { + reset_posvel_validity(); + + switch (custom_sub_mode) { + case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state); + break; + + case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: + if (_status_flags.condition_auto_mission_available) { + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state); + + } else { + main_ret = TRANSITION_DENIED; + } + + break; + + case PX4_CUSTOM_SUB_MODE_AUTO_RTL: + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, _internal_state); + break; + + case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, _status_flags, _internal_state); + break; + + case PX4_CUSTOM_SUB_MODE_AUTO_LAND: + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, _internal_state); + break; + + case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, _status_flags, + _internal_state); + break; + + case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND: + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, _status_flags, _internal_state); + break; + + default: + main_ret = TRANSITION_DENIED; + mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode"); + break; + } + + } else { + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state); + } + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { + /* ACRO */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { + /* STABILIZED */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + reset_posvel_validity(); + + /* OFFBOARD */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state); + } + + } else { + /* use base mode */ + if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state); + + } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { + /* POSCTL */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state); + + } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { + /* STABILIZED */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state); + + } else { + /* MANUAL */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); + } + } + } + + if (main_ret != TRANSITION_DENIED) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: { + + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints + // for logic state parameters + const int param1_arm = static_cast(roundf(cmd.param1)); + + if (param1_arm != 0 && param1_arm != 1) { + mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1); + + } else { + const bool cmd_arms = (param1_arm == 1); + + // Arm is forced (checks skipped) when param2 is set to a magic number. + const bool forced = (static_cast(roundf(cmd.param2)) == 21196); + + if (!forced) { + if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) { + if (cmd_arms) { + if (_armed.armed) { + mavlink_log_warning(&_mavlink_log_pub, "Arming denied! Already armed"); + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Not landed"); + } + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed"); + } + + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + break; + } + + const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); + + // Flick to in-air restore first if this comes from an onboard system and from IO + if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id + && cmd_from_io && cmd_arms) { + _status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; + } + } + + transition_result_t arming_res = TRANSITION_DENIED; + + if (cmd_arms) { + if (cmd.from_external) { + arming_res = arm(arm_disarm_reason_t::COMMAND_EXTERNAL); + + } else { + arming_res = arm(arm_disarm_reason_t::COMMAND_INTERNAL, !forced); + } + + } else { + if (cmd.from_external) { + arming_res = disarm(arm_disarm_reason_t::COMMAND_EXTERNAL); + + } else { + arming_res = disarm(arm_disarm_reason_t::COMMAND_INTERNAL); + } + } + + if (arming_res == TRANSITION_DENIED) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ + if (cmd_arms && (arming_res == TRANSITION_CHANGED) && + (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) { + + set_home_position(); + } + } + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { + if (cmd.param1 > 1.5f) { + // Test termination command triggers lockdown but not actual termination. + if (!_lockdown_triggered) { + _armed.lockdown = true; + _lockdown_triggered = true; + PX4_WARN("forcing lockdown (motors off)"); + } + + } else if (cmd.param1 > 0.5f) { + // Trigger real termination. + if (!_flight_termination_triggered) { + _armed.force_failsafe = true; + _flight_termination_triggered = true; + PX4_WARN("forcing failsafe (termination)"); + send_parachute_command(); + } + + } else { + _armed.force_failsafe = false; + _armed.lockdown = false; + + _lockdown_triggered = false; + _flight_termination_triggered = false; + + PX4_WARN("disabling failsafe and lockdown"); + } + + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + break; + + case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: { + bool use_current = cmd.param1 > 0.5f; + + if (use_current) { + /* use current position */ + if (set_home_position()) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } else { + const double lat = cmd.param5; + const double lon = cmd.param6; + const float alt = cmd.param7; + + if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) { + const vehicle_local_position_s &local_pos = _local_position_sub.get(); + + if (local_pos.xy_global && local_pos.z_global) { + /* use specified position */ + home_position_s home{}; + home.timestamp = hrt_absolute_time(); + + fillGlobalHomePos(home, lat, lon, alt); + + home.manual_home = true; + + // update local projection reference including altitude + struct map_projection_reference_s ref_pos; + map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon); + float home_x; + float home_y; + map_projection_project(&ref_pos, lat, lon, &home_x, &home_y); + const float home_z = -(alt - local_pos.ref_alt); + fillLocalHomePos(home, home_x, home_y, home_z, 0.f); + + /* mark home position as set */ + _status_flags.condition_home_position_valid = _home_pub.update(home); + + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: { + /* switch to RTL which ends the mission */ + if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, + _internal_state)) { + mavlink_log_info(&_mavlink_log_pub, "Returning to launch"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Return to launch denied"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { + /* ok, home set, use it to take off */ + if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, _status_flags, + _internal_state)) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Takeoff denied! Please disarm and retry"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { + if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, + _internal_state)) { + mavlink_log_info(&_mavlink_log_pub, "Landing at current position"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Landing denied! Please land manually"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { + if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, _status_flags, + _internal_state)) { + mavlink_log_info(&_mavlink_log_pub, "Precision landing"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Precision landing denied! Please land manually"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_MISSION_START: { + + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + + // check if current mission and first item are valid + if (_status_flags.condition_auto_mission_available) { + + // requested first mission item valid + if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { + + // switch to AUTO_MISSION and ARM + if ((TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, + _internal_state)) + && (TRANSITION_DENIED != arm(arm_disarm_reason_t::MISSION_START))) { + + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + mavlink_log_critical(&_mavlink_log_pub, "Mission start denied"); + } + } + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Mission start denied! No valid mission"); + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: { + // if no high latency telemetry exists send a failed acknowledge + if (_high_latency_datalink_heartbeat > _boot_timestamp) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED; + mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable"); + } + } + break; + + case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: + + // Switch to orbit state and let the orbit task handle the command further + if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags, + _internal_state)) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; + + case vehicle_command_s::VEHICLE_CMD_DO_MOTOR_TEST: + cmd_result = handle_command_motor_test(cmd); + break; + + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + const int param1 = cmd.param1; + + if (param1 == 0) { + // 0: Do nothing for autopilot + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + +#if defined(CONFIG_BOARDCTL_RESET) + + } else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) { + // 1: Reboot autopilot + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + while (1) { px4_usleep(1); } + +#endif // CONFIG_BOARDCTL_RESET + +#if defined(CONFIG_BOARDCTL_POWEROFF) + + } else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) { + // 2: Shutdown autopilot + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + while (1) { px4_usleep(1); } + +#endif // CONFIG_BOARDCTL_POWEROFF + +#if defined(CONFIG_BOARDCTL_RESET) + + } else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) { + // 3: Reboot autopilot and keep it in the bootloader until upgraded. + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + while (1) { px4_usleep(1); } + +#endif // CONFIG_BOARDCTL_RESET + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + } + } + + break; + + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + + if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) + || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) { + + // reject if armed or shutting down + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + + } else { + + /* try to go to INIT/PREFLIGHT arming state */ + if (TRANSITION_DENIED == arming_state_transition(_status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, _armed, + false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, + PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT + 30_s, // time since boot not relevant for switching to ARMING_STATE_INIT + (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)) + ) { + + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + break; + + } + + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::GyroCalibration); + + } else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || + (int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || + (int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + /* temperature calibration: handled in events module */ + break; + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::MagCalibration); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + /* disable RC control input completely */ + _status_flags.rc_input_blocked = true; + mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input"); + + } else if ((int)(cmd.param4) == 2) { + /* RC trim calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::RCTrimCalibration); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::AccelCalibration); + + } else if ((int)(cmd.param5) == 2) { + // board offset calibration + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::LevelCalibration); + + } else if ((int)(cmd.param5) == 4) { + // accelerometer quick calibration + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick); + + } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { + // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) + /* airspeed calibration */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _worker_thread.startTask(WorkerThread::Request::AirspeedCalibration); + + } else if ((int)(cmd.param7) == 1) { + /* do esc calibration */ + if (check_battery_disconnected(&_mavlink_log_pub)) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _status_flags.condition_calibration_enabled = true; + _armed.in_esc_calibration_mode = true; + _worker_thread.startTask(WorkerThread::Request::ESCCalibration); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + } + + } else if ((int)(cmd.param4) == 0) { + /* RC calibration ended - have we been in one worth confirming? */ + if (_status_flags.rc_input_blocked) { + /* enable RC control input */ + _status_flags.rc_input_blocked = false; + mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input"); + } + + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + } + } + + break; + } + + case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: { + // Magnetometer quick calibration using world magnetic model and known heading + if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) + || (_status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) + || _worker_thread.isBusy()) { + + // reject if armed or shutting down + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + // parameter 1: Heading (degrees) + // parameter 3: Latitude (degrees) + // parameter 4: Longitude (degrees) + + // assume vehicle pointing north (0 degrees) if heading isn't specified + const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f; + + float latitude = NAN; + float longitude = NAN; + + if (PX4_ISFINITE(cmd.param3) && PX4_ISFINITE(cmd.param4)) { + // invalid if both lat & lon are 0 (current mavlink spec) + if ((fabsf(cmd.param3) > 0) && (fabsf(cmd.param4) > 0)) { + latitude = cmd.param3; + longitude = cmd.param4; + } + } + + _status_flags.condition_calibration_enabled = true; + _worker_thread.setMagQuickData(heading_radians, latitude, longitude); + _worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick); + } + + break; + } + + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { + + if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) + || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN + || _worker_thread.isBusy()) { + + // reject if armed or shutting down + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + + } else { + + if (((int)(cmd.param1)) == 0) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _worker_thread.startTask(WorkerThread::Request::ParamLoadDefault); + + } else if (((int)(cmd.param1)) == 1) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _worker_thread.startTask(WorkerThread::Request::ParamSaveDefault); + + } else if (((int)(cmd.param1)) == 2) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + _worker_thread.startTask(WorkerThread::Request::ParamResetAll); + } + } + + break; + } + + case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: + case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: + case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN: + case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION: + case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL: + case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: + case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST: + case vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY: + case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL: + case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE: + case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM: + case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_FOCUS: + case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: + case vehicle_command_s::VEHICLE_CMD_DO_LAND_START: + case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND: + case vehicle_command_s::VEHICLE_CMD_LOGGING_START: + case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP: + case vehicle_command_s::VEHICLE_CMD_NAV_DELAY: + case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: + case vehicle_command_s::VEHICLE_CMD_NAV_ROI: + case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION: + case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: + case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: + case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE: + case vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN: + /* ignore commands that are handled by other parts of the system */ + break; + + default: + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + break; + } + + if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(cmd, cmd_result); + } + + return true; +} + +unsigned +Commander::handle_command_motor_test(const vehicle_command_s &cmd) +{ + if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { + return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + } + + if (_param_com_mot_test_en.get() != 1) { + return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; + } + + test_motor_s test_motor{}; + test_motor.timestamp = hrt_absolute_time(); + test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1; + + int throttle_type = (int)(cmd.param2 + 0.5f); + + if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT + return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + } + + int motor_count = (int)(cmd.param5 + 0.5); + + if (motor_count > 1) { + return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + } + + test_motor.action = test_motor_s::ACTION_RUN; + test_motor.value = math::constrain(cmd.param3 / 100.f, 0.f, 1.f); + + if (test_motor.value < FLT_EPSILON) { + // the message spec is not clear on whether 0 means stop, but it should be closer to what a user expects + test_motor.value = -1.f; + } + + test_motor.timeout_ms = (int)(cmd.param4 * 1000.f + 0.5f); + + // enforce a timeout and a maximum limit + if (test_motor.timeout_ms == 0 || test_motor.timeout_ms > 3000) { + test_motor.timeout_ms = 3000; + } + + test_motor.driver_instance = 0; // the mavlink command does not allow to specify the instance, so set to 0 for now + _test_motor_pub.publish(test_motor); + + return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; +} + +/** +* @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +bool +Commander::set_home_position() +{ + // Need global and local position fix to be able to set home + // but already set the home position in local coordinates if available + // in case the global position is only valid after takeoff + if (_status_flags.condition_local_position_valid) { + + // Set home position in local coordinates + const vehicle_local_position_s &lpos = _local_position_sub.get(); + _heading_reset_counter = lpos.heading_reset_counter; + + home_position_s home{}; + home.timestamp = hrt_absolute_time(); + home.manual_home = false; + fillLocalHomePos(home, lpos); + + if (_status_flags.condition_global_position_valid) { + + const vehicle_global_position_s &gpos = _global_position_sub.get(); + + // Ensure that the GPS accuracy is good enough for intializing home + if (isGPosGoodForInitializingHomePos(gpos)) { + fillGlobalHomePos(home, gpos); + setHomePosValid(); + } + } + + _home_pub.update(home); + } + + return _status_flags.condition_home_position_valid; +} + +bool +Commander::set_in_air_home_position() +{ + if (_status_flags.condition_local_position_valid + && _status_flags.condition_global_position_valid) { + + const vehicle_global_position_s &gpos = _global_position_sub.get(); + home_position_s home{}; + + // Ensure that the GPS accuracy is good enough for intializing home + if (isGPosGoodForInitializingHomePos(gpos)) { + home = _home_pub.get(); + home.timestamp = hrt_absolute_time(); + const vehicle_local_position_s &lpos = _local_position_sub.get(); + + if (_home_pub.get().valid_lpos) { + // Back-compute lon, lat and alt of home position given the home + // and current positions in local frame + map_projection_reference_s ref_pos; + double home_lat; + double home_lon; + map_projection_init(&ref_pos, gpos.lat, gpos.lon); + map_projection_reproject(&ref_pos, home.x - lpos.x, home.y - lpos.y, &home_lat, &home_lon); + const float home_alt = gpos.alt + home.z; + fillGlobalHomePos(home, home_lat, home_lon, home_alt); + + } else { + // Home position in local frame is unknowm, set + // home as current position + fillLocalHomePos(home, lpos); + fillGlobalHomePos(home, gpos); + } + + setHomePosValid(); + _home_pub.update(home); + } + } + + return _status_flags.condition_home_position_valid; +} + +bool +Commander::isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpos) const +{ + return (gpos.eph <= _param_com_home_h_t.get()) + && (gpos.epv <= _param_com_home_v_t.get()); +} + +void +Commander::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const +{ + fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, lpos.heading); +} + +void +Commander::fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) const +{ + home.x = x; + home.y = y; + home.z = z; + home.valid_lpos = true; + + home.yaw = heading; +} + +void Commander::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const +{ + fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt); +} + +void Commander::fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const +{ + home.lat = lat; + home.lon = lon; + home.valid_hpos = true; + home.alt = alt; + home.valid_alt = true; +} + +void Commander::setHomePosValid() +{ + // play tune first time we initialize HOME + if (!_status_flags.condition_home_position_valid) { + tune_home_set(true); + } + + // mark home position as set + _status_flags.condition_home_position_valid = true; +} + +bool +Commander::set_home_position_alt_only() +{ + const vehicle_local_position_s &lpos = _local_position_sub.get(); + + if (!_home_pub.get().valid_alt && lpos.z_global) { + // handle special case where we are setting only altitude using local position reference + home_position_s home{}; + home.alt = lpos.ref_alt; + home.valid_alt = true; + + home.timestamp = hrt_absolute_time(); + + return _home_pub.update(home); + } + + return false; +} + +void +Commander::updateHomePositionYaw(float yaw) +{ + home_position_s home = _home_pub.get(); + + home.yaw = yaw; + home.timestamp = hrt_absolute_time(); + + _home_pub.update(home); +} + +void +Commander::run() +{ + bool sensor_fail_tune_played = false; + + const param_t param_airmode = param_find("MC_AIRMODE"); + const param_t param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW"); + + /* initialize */ + led_init(); + buzzer_init(); + +#if defined(BOARD_HAS_POWER_CONTROL) + { + // we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen + // in IRQ context. + power_button_state_s button_state{}; + button_state.timestamp = hrt_absolute_time(); + button_state.event = 0xff; + power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state); + + _power_button_state_sub.copy(&button_state); + } + + if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) { + PX4_ERR("Failed to register power notification callback"); + } + +#endif // BOARD_HAS_POWER_CONTROL + + get_circuit_breaker_params(); + + bool param_init_forced = true; + + control_status_leds(true, _battery_warning); + + /* update vehicle status to find out vehicle type (required for preflight checks) */ + _status.system_type = _param_mav_type.get(); + + if (is_rotary_wing(_status) || is_vtol(_status)) { + _status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + } else if (is_fixed_wing(_status)) { + _status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + } else if (is_ground_rover(_status)) { + _status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + + } else { + _status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; + } + + _status.is_vtol = is_vtol(_status); + _status.is_vtol_tailsitter = is_vtol_tailsitter(_status); + + _boot_timestamp = hrt_absolute_time(); + + // initially set to failed + _last_lpos_fail_time_us = _boot_timestamp; + _last_gpos_fail_time_us = _boot_timestamp; + _last_lvel_fail_time_us = _boot_timestamp; + + int32_t airmode = 0; + int32_t rc_map_arm_switch = 0; + + _status.system_id = _param_mav_sys_id.get(); + arm_auth_init(&_mavlink_log_pub, &_status.system_id); + + // run preflight immediately to find all relevant parameters, but don't report + PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, false, + true, + hrt_elapsed_time(&_boot_timestamp)); + + while (!should_exit()) { + + /* update parameters */ + const bool params_updated = _parameter_update_sub.updated(); + + if (params_updated || param_init_forced) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); + + // update parameters from storage + updateParams(); + + // NAV_DLL_ACT value 4 Data Link Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10 + if (_param_nav_dll_act.get() == 4) { + mavlink_log_critical(&_mavlink_log_pub, "CASA Outback Challenge rules (NAV_DLL_ACT = 4) retired"); + _param_nav_dll_act.set(2); // value 2 Return mode + _param_nav_dll_act.commit_no_notification(); + } + + // NAV_RCL_ACT value 4 RC Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10 + if (_param_nav_rcl_act.get() == 4) { + mavlink_log_critical(&_mavlink_log_pub, "CASA Outback Challenge rules (NAV_RCL_ACT = 4) retired"); + _param_nav_rcl_act.set(2); // value 2 Return mode + _param_nav_rcl_act.commit_no_notification(); + } + + /* update parameters */ + if (!_armed.armed) { + _status.system_type = _param_mav_type.get(); + + const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) && _vtol_status.vtol_in_rw_mode); + const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) && !_vtol_status.vtol_in_rw_mode); + const bool is_ground = is_ground_rover(_status); + + /* disable manual override for all systems that rely on electronic stabilization */ + if (is_rotary) { + _status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + } else if (is_fixed) { + _status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + } else if (is_ground) { + _status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + } + + /* set vehicle_status.is_vtol flag */ + _status.is_vtol = is_vtol(_status); + _status.is_vtol_tailsitter = is_vtol_tailsitter(_status); + + /* check and update system / component ID */ + _status.system_id = _param_mav_sys_id.get(); + _status.component_id = _param_mav_comp_id.get(); + + get_circuit_breaker_params(); + + _status_changed = true; + } + + _status_flags.avoidance_system_required = _param_com_obs_avoid.get(); + + _status.rc_input_mode = _param_rc_in_off.get(); + + _arm_requirements.arm_authorization = _param_arm_auth_required.get(); + _arm_requirements.esc_check = _param_escs_checks_required.get(); + _arm_requirements.global_position = !_param_arm_without_gps.get(); + _arm_requirements.mission = _param_arm_mission_required.get(); + _arm_requirements.geofence = _param_geofence_action.get() > geofence_result_s::GF_ACTION_NONE; + + /* flight mode slots */ + _flight_mode_slots[0] = _param_fltmode_1.get(); + _flight_mode_slots[1] = _param_fltmode_2.get(); + _flight_mode_slots[2] = _param_fltmode_3.get(); + _flight_mode_slots[3] = _param_fltmode_4.get(); + _flight_mode_slots[4] = _param_fltmode_5.get(); + _flight_mode_slots[5] = _param_fltmode_6.get(); + + _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); + + /* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */ + if (param_airmode != PARAM_INVALID && param_rc_map_arm_switch != PARAM_INVALID) { + param_get(param_airmode, &airmode); + param_get(param_rc_map_arm_switch, &rc_map_arm_switch); + + if (airmode == 2 && rc_map_arm_switch == 0) { + airmode = 1; // change to roll/pitch airmode + param_set(param_airmode, &airmode); + mavlink_log_critical(&_mavlink_log_pub, "Yaw Airmode requires the use of an Arm Switch") + } + } + + _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1e6f); + + param_init_forced = false; + } + + /* Update OA parameter */ + _status_flags.avoidance_system_required = _param_com_obs_avoid.get(); + +#if defined(BOARD_HAS_POWER_CONTROL) + + /* handle power button state */ + if (_power_button_state_sub.updated()) { + power_button_state_s button_state; + + if (_power_button_state_sub.copy(&button_state)) { + if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { +#if defined(CONFIG_BOARDCTL_POWEROFF) + + if (shutdown_if_allowed() && (px4_shutdown_request() == 0)) { + while (1) { px4_usleep(1); } + } + +#endif // CONFIG_BOARDCTL_POWEROFF + } + } + } + +#endif // BOARD_HAS_POWER_CONTROL + + offboard_control_update(); + + if (_system_power_sub.updated()) { + system_power_s system_power{}; + _system_power_sub.copy(&system_power); + + if (hrt_elapsed_time(&system_power.timestamp) < 1_s) { + if (system_power.servo_valid && + !system_power.brick_valid && + !system_power.usb_connected) { + /* flying only on servo rail, this is unsafe */ + _status_flags.condition_power_input_valid = false; + + } else { + _status_flags.condition_power_input_valid = true; + } + +#if defined(CONFIG_BOARDCTL_RESET) + + if (!_status_flags.circuit_breaker_engaged_usb_check && _status_flags.usb_connected) { + /* if the USB hardware connection went away, reboot */ + if (_system_power_usb_connected && !system_power.usb_connected) { + /* + * Apparently the USB cable went away but we are still powered, + * so we bring the system back to a nominal state for flight. + * This is important to unload the USB stack of the OS which is + * a relatively complex piece of software that is non-essential + * for flight and continuing to run it would add a software risk + * without a need. The clean approach to unload it is to reboot. + */ + if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) { + mavlink_log_critical(&_mavlink_log_pub, "USB disconnected, rebooting for flight safety"); + + while (1) { px4_usleep(1); } + } + } + } + +#endif // CONFIG_BOARDCTL_RESET + + _system_power_usb_connected = system_power.usb_connected; + } + } + + /* Update land detector */ + if (_land_detector_sub.updated()) { + const bool was_landed = _land_detector.landed; + _land_detector_sub.copy(&_land_detector); + + // Only take actions if armed + if (_armed.armed) { + if (!was_landed && _land_detector.landed) { + mavlink_log_info(&_mavlink_log_pub, "Landing detected"); + _status.takeoff_time = 0; + + } else if (was_landed && !_land_detector.landed) { + mavlink_log_info(&_mavlink_log_pub, "Takeoff detected"); + _status.takeoff_time = hrt_absolute_time(); + _have_taken_off_since_arming = true; + + // Set all position and velocity test probation durations to takeoff value + // This is a larger value to give the vehicle time to complete a failsafe landing + // if faulty sensors cause loss of navigation shortly after takeoff. + _gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; + _lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; + _lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; + } + + // automatically set or update home position + if (!_home_pub.get().manual_home) { + // set the home position when taking off, but only if we were previously disarmed + // and at least 500 ms from commander start spent to avoid setting home on in-air restart + if (_should_set_home_on_takeoff && !_land_detector.landed && + (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { + if (was_landed) { + _should_set_home_on_takeoff = !set_home_position(); + + } else if (_param_com_home_in_air.get()) { + _should_set_home_on_takeoff = !set_in_air_home_position(); + } + } + } + } + } + + /* update safety topic */ + const bool safety_updated = _safety_sub.updated(); + + if (safety_updated) { + const bool previous_safety_valid = (_safety.timestamp != 0); + const bool previous_safety_off = _safety.safety_off; + + if (_safety_sub.copy(&_safety)) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, _safety.safety_switch_available, _safety.safety_off, + _safety.safety_switch_available, _status); + + // disarm if safety is now on and still armed + if (_armed.armed && _safety.safety_switch_available && !_safety.safety_off) { + + bool safety_disarm_allowed = (_status.hil_state == vehicle_status_s::HIL_STATE_OFF); + + // prevent disarming via safety button if not landed + if (hrt_elapsed_time(&_land_detector.timestamp) < 10_s) { + if (!_land_detector.landed) { + safety_disarm_allowed = false; + } + } + + if (safety_disarm_allowed) { + disarm(arm_disarm_reason_t::SAFETY_BUTTON); + } + } + + // Notify the user if the status of the safety switch changes + if (previous_safety_valid && _safety.safety_switch_available && previous_safety_off != _safety.safety_off) { + + if (_safety.safety_off) { + set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE); + + } else { + tune_neutral(true); + } + + _status_changed = true; + } + } + } + + /* update vtol vehicle status*/ + if (_vtol_vehicle_status_sub.updated()) { + /* vtol status changed */ + _vtol_vehicle_status_sub.copy(&_vtol_status); + _status.vtol_fw_permanent_stab = _vtol_status.fw_permanent_stab; + + /* Make sure that this is only adjusted if vehicle really is of type vtol */ + if (is_vtol(_status)) { + + // Check if there has been any change while updating the flags + const auto new_vehicle_type = _vtol_status.vtol_in_rw_mode ? + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : + vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + if (new_vehicle_type != _status.vehicle_type) { + _status.vehicle_type = _vtol_status.vtol_in_rw_mode ? + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : + vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + _status_changed = true; + } + + if (_status.in_transition_mode != _vtol_status.vtol_in_trans_mode) { + _status.in_transition_mode = _vtol_status.vtol_in_trans_mode; + _status_changed = true; + } + + if (_status.in_transition_to_fw != _vtol_status.in_transition_to_fw) { + _status.in_transition_to_fw = _vtol_status.in_transition_to_fw; + _status_changed = true; + } + + if (_status_flags.vtol_transition_failure != _vtol_status.vtol_transition_failsafe) { + _status_flags.vtol_transition_failure = _vtol_status.vtol_transition_failsafe; + _status_changed = true; + } + + const bool should_soft_stop = (_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + + if (_armed.soft_stop != should_soft_stop) { + _armed.soft_stop = should_soft_stop; + _status_changed = true; + } + } + } + + if (_esc_status_sub.updated()) { + /* ESCs status changed */ + esc_status_check(); + + } else if (_param_escs_checks_required.get() != 0) { + + if (!_status_flags.condition_escs_error) { + + if ((_last_esc_status_updated != 0) && (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms)) { + /* Detect timeout after first telemetry packet received + * Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that + */ + + mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout"); + _status_flags.condition_escs_error = true; + + } else if (_last_esc_status_updated == 0 && hrt_elapsed_time(&_boot_timestamp) > 5000_ms) { + /* Detect if esc telemetry is not connected after reboot */ + mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected "); + _status_flags.condition_escs_error = true; + } + } + } + + estimator_check(); + + // Auto disarm when landed or kill switch engaged + if (_armed.armed) { + + // Check for auto-disarm on landing or pre-flight + if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { + + if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming) { + _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s); + _auto_disarm_landed.set_state_and_update(_land_detector.landed, hrt_absolute_time()); + + } else if (_param_com_disarm_preflight.get() > 0 && !_have_taken_off_since_arming) { + _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); + _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); + } + + if (_auto_disarm_landed.get_state()) { + if (_have_taken_off_since_arming) { + disarm(arm_disarm_reason_t::AUTO_DISARM_LAND); + + } else { + disarm(arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT); + } + } + } + + // Auto disarm after 5 seconds if kill switch is engaged + bool auto_disarm = _armed.manual_lockdown; + + // auto disarm if locked down to avoid user confusion + // skipped in HITL where lockdown is enabled for safety + if (_status.hil_state != vehicle_status_s::HIL_STATE_ON) { + auto_disarm |= _armed.lockdown; + } + + _auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time()); + + if (_auto_disarm_killed.get_state()) { + if (_armed.manual_lockdown) { + disarm(arm_disarm_reason_t::KILL_SWITCH); + + } else { + disarm(arm_disarm_reason_t::LOCKDOWN); + } + } + + } else { + _auto_disarm_landed.set_state_and_update(false, hrt_absolute_time()); + _auto_disarm_killed.set_state_and_update(false, hrt_absolute_time()); + } + + if (_geofence_warning_action_on + && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL + && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER + && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { + + // reset flag again when we switched out of it + _geofence_warning_action_on = false; + } + + _cpuload_sub.update(&_cpuload); + + battery_status_check(); + + /* If in INIT state, try to proceed to STANDBY state */ + if (!_status_flags.condition_calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + + arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed, + true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, + _arm_requirements, hrt_elapsed_time(&_boot_timestamp), + arm_disarm_reason_t::TRANSITION_TO_STANDBY); + } + + /* start mission result check */ + if (_mission_result_sub.updated()) { + const mission_result_s &mission_result = _mission_result_sub.get(); + + const auto prev_mission_instance_count = mission_result.instance_count; + _mission_result_sub.update(); + + // if mission_result is valid for the current mission + const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp) + && (mission_result.instance_count > 0); + + _status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid; + + if (mission_result_ok) { + if (_status.mission_failure != mission_result.failure) { + _status.mission_failure = mission_result.failure; + _status_changed = true; + + if (_status.mission_failure) { + mavlink_log_critical(&_mavlink_log_pub, "Mission cannot be completed"); + } + } + + /* Only evaluate mission state if home is set */ + if (_status_flags.condition_home_position_valid && + (prev_mission_instance_count != mission_result.instance_count)) { + + if (!_status_flags.condition_auto_mission_available) { + /* the mission is invalid */ + tune_mission_fail(true); + + } else if (mission_result.warning) { + /* the mission has a warning */ + tune_mission_warn(true); + + } else { + /* the mission is valid */ + tune_mission_ok(true); + } + } + } + + // Transition main state to loiter or auto-mission after takeoff is completed. + if (_armed.armed && !_land_detector.landed + && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) + && (mission_result.timestamp >= _status.nav_state_timestamp) + && mission_result.finished) { + + if ((_param_takeoff_finished_action.get() == 1) && _status_flags.condition_auto_mission_available) { + main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state); + + } else { + main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state); + } + } + } + + /* start geofence result check */ + _geofence_result_sub.update(&_geofence_result); + _status.geofence_violated = _geofence_result.geofence_violated; + + const bool in_low_battery_failsafe = _battery_warning > battery_status_s::BATTERY_WARNING_LOW; + + // Geofence actions + const bool geofence_action_enabled = _geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE; + + if (_armed.armed + && geofence_action_enabled + && !in_low_battery_failsafe) { + + // check for geofence violation transition + if (_geofence_result.geofence_violated && !_geofence_violated_prev) { + + switch (_geofence_result.geofence_action) { + case (geofence_result_s::GF_ACTION_NONE) : { + // do nothing + break; + } + + case (geofence_result_s::GF_ACTION_WARN) : { + // do nothing, mavlink critical messages are sent by navigator + break; + } + + case (geofence_result_s::GF_ACTION_LOITER) : { + if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, + _internal_state)) { + _geofence_loiter_on = true; + } + + break; + } + + case (geofence_result_s::GF_ACTION_RTL) : { + if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, + _internal_state)) { + _geofence_rtl_on = true; + } + + break; + } + + case (geofence_result_s::GF_ACTION_LAND) : { + if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, + _internal_state)) { + _geofence_land_on = true; + } + + break; + } + + case (geofence_result_s::GF_ACTION_TERMINATE) : { + PX4_WARN("Flight termination because of geofence"); + + if (!_flight_termination_triggered && !_lockdown_triggered) { + _flight_termination_triggered = true; + mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated"); + _armed.force_failsafe = true; + _status_changed = true; + send_parachute_command(); + } + + break; + } + } + } + + _geofence_violated_prev = _geofence_result.geofence_violated; + + // reset if no longer in LOITER or if manually switched to LOITER + const bool in_loiter_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER; + const bool manual_loiter_switch_on = _manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON; + + if (!in_loiter_mode || manual_loiter_switch_on) { + _geofence_loiter_on = false; + } + + + // reset if no longer in RTL or if manually switched to RTL + const bool in_rtl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL; + const bool manual_return_switch_on = _manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON; + + if (!in_rtl_mode || manual_return_switch_on) { + _geofence_rtl_on = false; + } + + // reset if no longer in LAND or if manually switched to LAND + const bool in_land_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND; + + if (!in_land_mode) { + _geofence_land_on = false; + } + + _geofence_warning_action_on = _geofence_warning_action_on || (_geofence_loiter_on || _geofence_rtl_on + || _geofence_land_on); + + } else { + // No geofence checks, reset flags + _geofence_loiter_on = false; + _geofence_rtl_on = false; + _geofence_land_on = false; + _geofence_warning_action_on = false; + _geofence_violated_prev = false; + } + + /* Check for mission flight termination */ + if (_armed.armed && _mission_result_sub.get().flight_termination && + !_status_flags.circuit_breaker_flight_termination_disabled) { + + + if (!_flight_termination_triggered && !_lockdown_triggered) { + mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated"); + _flight_termination_triggered = true; + _armed.force_failsafe = true; + _status_changed = true; + send_parachute_command(); + } + + if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + mavlink_log_critical(&_mavlink_log_pub, "Flight termination active"); + } + } + + // Manual control input handling + _manual_control.setRCAllowed(!_status_flags.rc_input_blocked); + + if (_manual_control.update()) { + + /* handle the case where RC signal was regained */ + if (!_status_flags.rc_signal_found_once) { + _status_flags.rc_signal_found_once = true; + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _status_flags.rc_calibration_valid, _status); + _status_changed = true; + + } else { + if (_status.rc_signal_lost) { + if (_rc_signal_lost_timestamp > 0) { + mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs", + hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6); + } + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _status_flags.rc_calibration_valid, _status); + _status_changed = true; + } + } + + _status.rc_signal_lost = false; + + const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF); + + if (rc_arming_enabled) { + if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) { + disarm(arm_disarm_reason_t::RC_STICK); + } + + if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) { + if (_vehicle_control_mode.flag_control_manual_enabled) { + arm(arm_disarm_reason_t::RC_STICK); + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first"); + tune_negative(true); + } + } + } + + // abort autonomous mode and switch to position mode if sticks are moved significantly + if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && !in_low_battery_failsafe && !_geofence_warning_action_on + && _manual_control.wantsOverride(_vehicle_control_mode, _status)) { + if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, + _internal_state) == TRANSITION_CHANGED) { + tune_positive(true); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks"); + _status_changed = true; + + } else if (main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, + _internal_state) == TRANSITION_CHANGED) { + tune_positive(true); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks"); + _status_changed = true; + } + } + + if (!_armed.armed && _manual_control.isMavlink() && (_internal_state.main_state_changes == 0)) { + // if there's never been a mode change force position control as initial state + _internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; + } + + if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) { + + // handle landing gear switch if configured and in a manual mode + if ((_vehicle_control_mode.flag_control_manual_enabled) && + (_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && + (_last_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && + (_manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) { + // Only switch the landing gear up if the user switched from gear down to gear up. + int8_t gear = landing_gear_s::GEAR_KEEP; + + if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { + gear = landing_gear_s::GEAR_DOWN; + + } else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { + // gear up ignored unless flying + if (!_land_detector.landed && !_land_detector.maybe_landed) { + gear = landing_gear_s::GEAR_UP; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") + } + } + + if (gear != landing_gear_s::GEAR_KEEP) { + landing_gear_s landing_gear{}; + landing_gear.landing_gear = gear; + landing_gear.timestamp = hrt_absolute_time(); + _landing_gear_pub.publish(landing_gear); + } + } + + // evaluate the main state machine according to mode switches + if (set_main_state(_status_changed) == TRANSITION_CHANGED) { + // play tune on mode change only if armed, blink LED always + tune_positive(_armed.armed); + _status_changed = true; + } + } + + /* check throttle kill switch */ + if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { + /* set lockdown flag */ + if (!_armed.manual_lockdown) { + const char kill_switch_string[] = "Kill-switch engaged"; + + if (_land_detector.landed) { + mavlink_log_info(&_mavlink_log_pub, kill_switch_string); + + } else { + mavlink_log_critical(&_mavlink_log_pub, kill_switch_string); + } + + _status_changed = true; + _armed.manual_lockdown = true; + } + + } else if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) { + if (_armed.manual_lockdown) { + mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged"); + _status_changed = true; + _armed.manual_lockdown = false; + } + } + + /* no else case: do not change lockdown flag in unconfigured case */ + } + + if (!_manual_control.isRCAvailable()) { + // set RC lost + if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) { + // ignore RC lost during calibration + if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) { + mavlink_log_critical(&_mavlink_log_pub, "Manual control lost"); + _status.rc_signal_lost = true; + _rc_signal_lost_timestamp = _manual_control.getLastRcTimestamp(); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status); + _status_changed = true; + } + } + } + + // data link checks which update the status + data_link_check(); + + avoidance_check(); + + // engine failure detection + // TODO: move out of commander + if (_actuator_controls_sub.updated()) { + /* Check engine failure + * only for fixed wing for now + */ + if (!_status_flags.circuit_breaker_engaged_enginefailure_check && + _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol && _armed.armed) { + + actuator_controls_s actuator_controls{}; + _actuator_controls_sub.copy(&actuator_controls); + + const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + const float current2throttle = _battery_current / throttle; + + if (((throttle > _param_ef_throttle_thres.get()) && (current2throttle < _param_ef_current2throttle_thres.get())) + || _status.engine_failure) { + + const float elapsed = hrt_elapsed_time(&_timestamp_engine_healthy) / 1e6f; + + /* potential failure, measure time */ + if ((_timestamp_engine_healthy > 0) && (elapsed > _param_ef_time_thres.get()) + && !_status.engine_failure) { + + _status.engine_failure = true; + _status_changed = true; + + PX4_ERR("Engine Failure"); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, _status); + } + } + + } else { + /* no failure reset flag */ + _timestamp_engine_healthy = hrt_absolute_time(); + + if (_status.engine_failure) { + _status.engine_failure = false; + _status_changed = true; + } + } + } + + /* check if we are disarmed and there is a better mode to wait in */ + if (!_armed.armed) { + /* if there is no radio control but GPS lock the user might want to fly using + * just a tablet. Since the RC will force its mode switch setting on connecting + * we can as well just wait in a hold mode which enables tablet control. + */ + if (_status.rc_signal_lost && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL) + && _status_flags.condition_global_position_valid) { + + main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state); + } + } + + /* handle commands last, as the system needs to be updated to handle them */ + while (_cmd_sub.updated()) { + /* got command */ + const unsigned last_generation = _cmd_sub.get_last_generation(); + vehicle_command_s cmd; + + if (_cmd_sub.copy(&cmd)) { + if (_cmd_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _cmd_sub.get_last_generation()); + } + + if (handle_command(cmd)) { + _status_changed = true; + } + } + } + + /* Check for failure detector status */ + if (_failure_detector.update(_status, _vehicle_control_mode)) { + _status.failure_detector_status = _failure_detector.getStatus(); + _status_changed = true; + + if (_armed.armed) { + if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { + // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs + if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { + disarm(arm_disarm_reason_t::FAILURE_DETECTOR); + mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request"); + } + } + + if (_status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | + vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) { + const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get()); + + if (is_right_after_takeoff && !_lockdown_triggered) { + // This handles the case where something fails during the early takeoff phase + _armed.lockdown = true; + _lockdown_triggered = true; + mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown"); + + } else if (!_status_flags.circuit_breaker_flight_termination_disabled && + !_flight_termination_triggered && !_lockdown_triggered) { + + _armed.force_failsafe = true; + _flight_termination_triggered = true; + mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight"); + send_parachute_command(); + } + } + } + } + + /* Get current timestamp */ + const hrt_abstime now = hrt_absolute_time(); + + // automatically set or update home position + if (!_home_pub.get().manual_home) { + const vehicle_local_position_s &local_position = _local_position_sub.get(); + + if (!_armed.armed) { + if (_home_pub.get().valid_lpos) { + if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) { + /* distance from home */ + float home_dist_xy = -1.0f; + float home_dist_z = -1.0f; + mavlink_wpm_distance_to_point_local(_home_pub.get().x, _home_pub.get().y, _home_pub.get().z, + local_position.x, local_position.y, local_position.z, + &home_dist_xy, &home_dist_z); + + if ((home_dist_xy > local_position.eph * 2.0f) || (home_dist_z > local_position.epv * 2.0f)) { + + /* update when disarmed, landed and moved away from current home position */ + set_home_position(); + } + } + + } else { + /* First time home position update - but only if disarmed */ + set_home_position(); + + /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin. + * This allows home altitude to be used in the calculation of height above takeoff location when GPS + * use has commenced after takeoff. */ + if (!_status_flags.condition_home_position_valid) { + set_home_position_alt_only(); + } + } + } + } + + // check for arming state change + if (_was_armed != _armed.armed) { + _status_changed = true; + + if (_armed.armed) { + if (!_land_detector.landed) { // check if takeoff already detected upon arming + _have_taken_off_since_arming = true; + } + + } else { // increase the flight uuid upon disarming + const int32_t flight_uuid = _param_flight_uuid.get() + 1; + _param_flight_uuid.set(flight_uuid); + _param_flight_uuid.commit_no_notification(); + + _last_disarmed_timestamp = hrt_absolute_time(); + + _should_set_home_on_takeoff = true; + } + } + + if (!_armed.armed) { + /* Reset the flag if disarmed. */ + _have_taken_off_since_arming = false; + } + + /* now set navigation state according to failsafe and main state */ + bool nav_state_changed = set_nav_state(_status, + _armed, + _internal_state, + &_mavlink_log_pub, + static_cast(_param_nav_dll_act.get()), + _mission_result_sub.get().finished, + _mission_result_sub.get().stay_in_failsafe, + _status_flags, + _land_detector.landed, + static_cast(_param_nav_rcl_act.get()), + static_cast(_param_com_obl_act.get()), + static_cast(_param_com_obl_rc_act.get()), + static_cast(_param_com_posctl_navl.get()), + _param_com_rcl_act_t.get(), + _param_com_rcl_except.get()); + + if (nav_state_changed) { + _status.nav_state_timestamp = hrt_absolute_time(); + } + + if (_status.failsafe != _failsafe_old) { + _status_changed = true; + + if (_status.failsafe) { + mavlink_log_info(&_mavlink_log_pub, "Failsafe mode activated"); + + } else { + mavlink_log_info(&_mavlink_log_pub, "Failsafe mode deactivated"); + } + + _failsafe_old = _status.failsafe; + } + + /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 2 Hz or immediately when changed */ + if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed) { + + update_control_mode(); + + _status.timestamp = hrt_absolute_time(); + _status_pub.publish(_status); + + switch ((PrearmedMode)_param_com_prearm_mode.get()) { + case PrearmedMode::DISABLED: + /* skip prearmed state */ + _armed.prearmed = false; + break; + + case PrearmedMode::ALWAYS: + /* safety is not present, go into prearmed + * (all output drivers should be started / unlocked last in the boot process + * when the rest of the system is fully initialized) + */ + _armed.prearmed = (hrt_elapsed_time(&_boot_timestamp) > 5_s); + break; + + case PrearmedMode::SAFETY_BUTTON: + if (_safety.safety_switch_available) { + /* safety switch is present, go into prearmed if safety is off */ + _armed.prearmed = _safety.safety_off; + + } else { + /* safety switch is not present, do not go into prearmed */ + _armed.prearmed = false; + } + + break; + + default: + _armed.prearmed = false; + break; + } + + _armed.timestamp = hrt_absolute_time(); + _armed_pub.publish(_armed); + + /* publish internal state for logging purposes */ + _internal_state.timestamp = hrt_absolute_time(); + _commander_state_pub.publish(_internal_state); + + // Evaluate current prearm status + if (!_armed.armed && !_status_flags.condition_calibration_enabled) { + bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true, + hrt_elapsed_time(&_boot_timestamp)); + + // skip arm authorization check until actual arming attempt + PreFlightCheck::arm_requirements_t arm_req = _arm_requirements; + arm_req.arm_authorization = false; + bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _safety, arm_req, _status, false); + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res + && prearm_check_res), _status); + } + + /* publish vehicle_status_flags */ + _status_flags.timestamp = hrt_absolute_time(); + _vehicle_status_flags_pub.publish(_status_flags); + } + + /* play arming and battery warning tunes */ + if (!_arm_tune_played && _armed.armed && + (_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) { + + /* play tune when armed */ + set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); + _arm_tune_played = true; + + } else if (!_status_flags.usb_connected && + (_status.hil_state != vehicle_status_s::HIL_STATE_ON) && + (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { + /* play tune on battery critical */ + set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); + + } else if ((_status.hil_state != vehicle_status_s::HIL_STATE_ON) && + (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) { + /* play tune on battery warning */ + set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); + + } else if (_status.failsafe) { + tune_failsafe(true); + + } else { + set_tune(tune_control_s::TUNE_ID_STOP); + } + + /* reset arm_tune_played when disarmed */ + if (!_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { + + // Notify the user that it is safe to approach the vehicle + if (_arm_tune_played) { + tune_neutral(true); + } + + _arm_tune_played = false; + } + + /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ + _status_flags.condition_system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); + + if (!sensor_fail_tune_played && (!_status_flags.condition_system_sensors_initialized + && _status_flags.condition_system_hotplug_timeout)) { + + set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING); + sensor_fail_tune_played = true; + _status_changed = true; + } + + _counter++; + + int blink_state = blink_msg_state(); + + if (blink_state > 0) { + /* blinking LED message, don't touch LEDs */ + if (blink_state == 2) { + /* blinking LED message completed, restore normal state */ + control_status_leds(true, _battery_warning); + } + + } else { + /* normal state */ + control_status_leds(_status_changed, _battery_warning); + } + + // check if the worker has finished + if (_worker_thread.hasResult()) { + int ret = _worker_thread.getResultAndReset(); + _armed.in_esc_calibration_mode = false; + + if (_status_flags.condition_calibration_enabled) { // did we do a calibration? + _status_flags.condition_calibration_enabled = false; + + if (ret == 0) { + tune_positive(true); + + } else { + tune_negative(true); + } + } + } + + _status_changed = false; + + /* store last position lock state */ + _last_condition_local_altitude_valid = _status_flags.condition_local_altitude_valid; + _last_condition_local_position_valid = _status_flags.condition_local_position_valid; + _last_condition_global_position_valid = _status_flags.condition_global_position_valid; + + _was_armed = _armed.armed; + + arm_auth_update(now, params_updated || param_init_forced); + + px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed); + + px4_usleep(COMMANDER_MONITORING_INTERVAL); + } + + rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF); + + /* close fds */ + led_deinit(); + buzzer_deinit(); +} + +void +Commander::get_circuit_breaker_params() +{ + _status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), + CBRK_SUPPLY_CHK_KEY); + _status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(), + CBRK_USB_CHK_KEY); + _status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(), + CBRK_AIRSPD_CHK_KEY); + _status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled_by_val(_param_cbrk_enginefail.get(), + CBRK_ENGINEFAIL_KEY); + _status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), + CBRK_FLIGHTTERM_KEY); + _status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled_by_val(_param_cbrk_velposerr.get(), + CBRK_VELPOSERR_KEY); + _status_flags.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val(_param_cbrk_vtolarming.get(), + CBRK_VTOLARMING_KEY); +} + +void +Commander::control_status_leds(bool changed, const uint8_t battery_warning) +{ + bool overload = (_cpuload.load > 0.95f) || (_cpuload.ram_usage > 0.98f); + + if (_overload_start == 0 && overload) { + _overload_start = hrt_absolute_time(); + + } else if (!overload) { + _overload_start = 0; + } + + // driving the RGB led + if (changed || _last_overload != overload) { + uint8_t led_mode = led_control_s::MODE_OFF; + uint8_t led_color = led_control_s::COLOR_WHITE; + bool set_normal_color = false; + + uint64_t overload_warn_delay = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms; + + /* set mode */ + if (overload && (hrt_elapsed_time(&_overload_start) > overload_warn_delay)) { + led_mode = led_control_s::MODE_BLINK_FAST; + led_color = led_control_s::COLOR_PURPLE; + + } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + led_mode = led_control_s::MODE_ON; + set_normal_color = true; + + } else if (!_status_flags.condition_system_sensors_initialized && _status_flags.condition_system_hotplug_timeout) { + led_mode = led_control_s::MODE_BLINK_FAST; + led_color = led_control_s::COLOR_RED; + + } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + led_mode = led_control_s::MODE_BREATHE; + set_normal_color = true; + + } else if (!_status_flags.condition_system_sensors_initialized && !_status_flags.condition_system_hotplug_timeout) { + led_mode = led_control_s::MODE_BREATHE; + set_normal_color = true; + + } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + // if in init status it should not be in the error state + led_mode = led_control_s::MODE_OFF; + + } else { // STANDBY_ERROR and other states + led_mode = led_control_s::MODE_BLINK_NORMAL; + led_color = led_control_s::COLOR_RED; + } + + if (set_normal_color) { + /* set color */ + if (_status.failsafe) { + led_color = led_control_s::COLOR_PURPLE; + + } else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) { + led_color = led_control_s::COLOR_AMBER; + + } else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) { + led_color = led_control_s::COLOR_RED; + + } else { + if (_status_flags.condition_home_position_valid && _status_flags.condition_global_position_valid) { + led_color = led_control_s::COLOR_GREEN; + + } else { + led_color = led_control_s::COLOR_BLUE; + } + } + } + + if (led_mode != led_control_s::MODE_OFF) { + rgbled_set_color_and_mode(led_color, led_mode); + } + } + + _last_overload = overload; + +#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS) + + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ + if (_armed.armed) { + if (_status.failsafe) { + BOARD_ARMED_LED_OFF(); + + if (_leds_counter % 5 == 0) { + BOARD_ARMED_STATE_LED_TOGGLE(); + } + + } else { + BOARD_ARMED_STATE_LED_OFF(); + + /* armed, solid */ + BOARD_ARMED_LED_ON(); + } + + } else if (_armed.ready_to_arm) { + BOARD_ARMED_LED_OFF(); + + /* ready to arm, blink at 1Hz */ + if (_leds_counter % 20 == 0) { + BOARD_ARMED_STATE_LED_TOGGLE(); + } + + } else { + BOARD_ARMED_LED_OFF(); + + /* not ready to arm, blink at 10Hz */ + if (_leds_counter % 2 == 0) { + BOARD_ARMED_STATE_LED_TOGGLE(); + } + } + +#endif + + /* give system warnings on error LED */ + if (overload) { + if (_leds_counter % 2 == 0) { + BOARD_OVERLOAD_LED_TOGGLE(); + } + + } else { + BOARD_OVERLOAD_LED_OFF(); + } + + _leds_counter++; +} + +transition_result_t +Commander::set_main_state(bool &changed) +{ + if (_safety.override_available && _safety.override_enabled) { + return set_main_state_override_on(changed); + + } else { + return set_main_state_rc(); + } +} + +transition_result_t +Commander::set_main_state_override_on(bool &changed) +{ + const transition_result_t res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, + _internal_state); + changed = (res == TRANSITION_CHANGED); + + return res; +} + +transition_result_t +Commander::set_main_state_rc() +{ + if ((_manual_control_switches.timestamp == 0) + || (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) { + + // no manual control or no update -> nothing changed + return TRANSITION_NOT_CHANGED; + } + + // Note: even if _status_flags.offboard_control_set_by_command is set + // we want to allow rc mode change to take precedence. This is a safety + // feature, just in case offboard control goes crazy. + + // only switch mode based on RC switch if necessary to also allow mode switching via MAVLink + bool should_evaluate_rc_mode_switch = + (_last_manual_control_switches.offboard_switch != _manual_control_switches.offboard_switch) + || (_last_manual_control_switches.return_switch != _manual_control_switches.return_switch) + || (_last_manual_control_switches.mode_switch != _manual_control_switches.mode_switch) + || (_last_manual_control_switches.acro_switch != _manual_control_switches.acro_switch) + || (_last_manual_control_switches.posctl_switch != _manual_control_switches.posctl_switch) + || (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch) + || (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot) + || (_last_manual_control_switches.stab_switch != _manual_control_switches.stab_switch) + || (_last_manual_control_switches.man_switch != _manual_control_switches.man_switch); + + + if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + // if already armed don't evaluate first time RC + if (_last_manual_control_switches.timestamp == 0) { + should_evaluate_rc_mode_switch = false; + _last_manual_control_switches = _manual_control_switches; + } + + } else { + // not armed + if (!should_evaluate_rc_mode_switch) { + // to respect initial switch position (eg POSCTL) force RC switch re-evaluation if estimates become valid + const bool altitude_got_valid = (!_last_condition_local_altitude_valid && _status_flags.condition_local_altitude_valid); + const bool lpos_got_valid = (!_last_condition_local_position_valid && _status_flags.condition_local_position_valid); + const bool gpos_got_valid = (!_last_condition_global_position_valid && _status_flags.condition_global_position_valid); + + if (altitude_got_valid || lpos_got_valid || gpos_got_valid) { + should_evaluate_rc_mode_switch = true; + } + } + } + + if (!should_evaluate_rc_mode_switch) { + /* no timestamp change or no switch change -> nothing changed */ + return TRANSITION_NOT_CHANGED; + } + + _last_manual_control_switches = _manual_control_switches; + + // reset the position and velocity validity calculation to give the best change of being able to select + // the desired mode + reset_posvel_validity(); + + /* set main state according to RC switches */ + transition_result_t res = TRANSITION_NOT_CHANGED; + + /* offboard switch overrides main switch */ + if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state); + + if (res == TRANSITION_DENIED) { + print_reject_mode(commander_state_s::MAIN_STATE_OFFBOARD); + /* mode rejected, continue to evaluate the main system mode */ + + } else { + /* changed successfully or already in this state */ + return res; + } + } + + /* RTL switch overrides main switch */ + if (_manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { + res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_RTL); + + if (res != TRANSITION_DENIED) { + /* changed successfully or already in this state */ + return res; + } + + /* if we get here mode was rejected, continue to evaluate the main system mode */ + } + + /* Loiter switch overrides main switch */ + if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state); + + if (res == TRANSITION_DENIED) { + print_reject_mode(commander_state_s::MAIN_STATE_AUTO_LOITER); + /* mode rejected, continue to evaluate the main system mode */ + + } else { + /* changed successfully or already in this state */ + return res; + } + } + + /* we know something has changed - check if we are in mode slot operation */ + if (_manual_control_switches.mode_slot != manual_control_switches_s::MODE_SLOT_NONE) { + + if (_manual_control_switches.mode_slot > manual_control_switches_s::MODE_SLOT_NUM) { + PX4_WARN("m slot overflow"); + return TRANSITION_DENIED; + } + + int new_mode = _flight_mode_slots[_manual_control_switches.mode_slot - 1]; + + if (new_mode < 0) { + /* slot is unused */ + res = TRANSITION_NOT_CHANGED; + + } else { + res = try_mode_change(new_mode); + } + + return res; + + } else if (_manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) { + /* offboard and RTL switches off or denied, check main mode switch */ + switch (_manual_control_switches.mode_switch) { + case manual_control_switches_s::SWITCH_POS_NONE: + res = TRANSITION_NOT_CHANGED; + break; + + case manual_control_switches_s::SWITCH_POS_OFF: // MANUAL + if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_NONE && + _manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) { + /* + * Legacy mode: + * Acro switch being used as stabilized switch in FW. + */ + if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) { + /* manual mode is stabilized already for multirotors, so switch to acro + * for any non-manual mode + */ + if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_status.is_vtol) { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state); + + } else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state); + + } else { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); + } + + } else { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); + } + + } else { + /* New mode: + * - Acro is Acro + * - Manual is not default anymore when the manual switch is assigned + */ + if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_ON) { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); + + } else if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state); + + } else if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) { + res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state); + + } else if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) { + // default to MANUAL when no manual switch is set + res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); + + } else { + // default to STAB when the manual switch is assigned (but off) + res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state); + } + } + + // TRANSITION_DENIED is not possible here + break; + + case manual_control_switches_s::SWITCH_POS_MIDDLE: // ASSIST + if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) { + res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL); + + } else { + res = try_mode_change(commander_state_s::MAIN_STATE_ALTCTL); + } + + break; + + case manual_control_switches_s::SWITCH_POS_ON: // AUTO + res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION); + break; + + default: + break; + } + + } + + return res; +} + +void +Commander::reset_posvel_validity() +{ + // reset all the check probation times back to the minimum value + _gpos_probation_time_us = POSVEL_PROBATION_MIN; + _lpos_probation_time_us = POSVEL_PROBATION_MIN; + _lvel_probation_time_us = POSVEL_PROBATION_MIN; + + // recheck validity + UpdateEstimateValidity(); +} + +bool +Commander::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) +{ + bool valid = was_valid; + + // constrain probation times + if (_land_detector.landed) { + *probation_time_us = POSVEL_PROBATION_MIN; + } + + const bool data_stale = ((hrt_elapsed_time(&data_timestamp_us) > _param_com_pos_fs_delay.get() * 1_s) + || (data_timestamp_us == 0)); + const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy); + + const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy); + + // Check accuracy with hysteresis in both test level and time + if (level_check_pass) { + if (was_valid) { + // still valid, continue to decrease probation time + const int64_t probation_time_new = *probation_time_us - hrt_elapsed_time(last_fail_time_us); + *probation_time_us = math::constrain(probation_time_new, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX); + + } else { + // check if probation period has elapsed + if (hrt_elapsed_time(last_fail_time_us) > *probation_time_us) { + valid = true; + } + } + + } else { + // level check failed + if (was_valid) { + // FAILURE! no longer valid + valid = false; + + } else { + // failed again, increase probation time + const int64_t probation_time_new = *probation_time_us + hrt_elapsed_time(last_fail_time_us) * + _param_com_pos_fs_gain.get(); + *probation_time_us = math::constrain(probation_time_new, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX); + } + + *last_fail_time_us = hrt_absolute_time(); + } + + if (was_valid != valid) { + _status_changed = true; + } + + return valid; +} + +void +Commander::update_control_mode() +{ + _vehicle_control_mode = {}; + + /* set vehicle_control_mode according to set_navigation_state */ + _vehicle_control_mode.flag_armed = _armed.armed; + + _vehicle_control_mode.flag_external_manual_override_ok = + (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol); + + switch (_status.nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = stabilization_required(); + _vehicle_control_mode.flag_control_attitude_enabled = stabilization_required(); + break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + break; + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + /* override is not ok for the RTL and recovery mode */ + _vehicle_control_mode.flag_external_manual_override_ok = false; + + /* fallthrough */ + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + _vehicle_control_mode.flag_control_auto_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + break; + + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + _vehicle_control_mode.flag_control_auto_enabled = false; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + break; + + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + /* disable all controllers on termination */ + _vehicle_control_mode.flag_control_termination_enabled = true; + break; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + _vehicle_control_mode.flag_control_offboard_enabled = true; + + if (_offboard_control_mode_sub.get().position) { + _vehicle_control_mode.flag_control_position_enabled = true; + _vehicle_control_mode.flag_control_velocity_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_acceleration_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + + } else if (_offboard_control_mode_sub.get().velocity) { + _vehicle_control_mode.flag_control_velocity_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_acceleration_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + + } else if (_offboard_control_mode_sub.get().acceleration) { + _vehicle_control_mode.flag_control_acceleration_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + + } else if (_offboard_control_mode_sub.get().attitude) { + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + + } else if (_offboard_control_mode_sub.get().body_rate) { + _vehicle_control_mode.flag_control_rates_enabled = true; + } + + break; + + case vehicle_status_s::NAVIGATION_STATE_ORBIT: + _vehicle_control_mode.flag_control_manual_enabled = false; + _vehicle_control_mode.flag_control_auto_enabled = false; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + break; + + default: + break; + } + + _vehicle_control_mode.flag_multicopter_position_control_enabled = + (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && (_vehicle_control_mode.flag_control_altitude_enabled + || _vehicle_control_mode.flag_control_climb_rate_enabled + || _vehicle_control_mode.flag_control_position_enabled + || _vehicle_control_mode.flag_control_velocity_enabled + || _vehicle_control_mode.flag_control_acceleration_enabled); + + _vehicle_control_mode.timestamp = hrt_absolute_time(); + _control_mode_pub.publish(_vehicle_control_mode); +} + +bool +Commander::stabilization_required() +{ + return (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || // is a rotary wing, or + _status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or + (_vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND + _status.vehicle_type == + vehicle_status_s::VEHICLE_TYPE_FIXED_WING)); // is a fixed wing, ie: transitioning back to rotary wing mode +} + +void +Commander::print_reject_mode(uint8_t main_state) +{ + if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) { + + mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available.", main_state_str(main_state)); + + /* only buzz if armed, because else we're driving people nuts indoors + they really need to look at the leds as well. */ + tune_negative(_armed.armed); + + _last_print_mode_reject_time = hrt_absolute_time(); + } +} + +void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result) +{ + switch (result) { + case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: + break; + + case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED: + tune_negative(true); + break; + + case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED: + tune_negative(true); + break; + + case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + tune_negative(true); + break; + + case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED: + tune_negative(true); + break; + + default: + break; + } + + /* publish ACK */ + vehicle_command_ack_s command_ack{}; + command_ack.command = cmd.command; + command_ack.result = result; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + command_ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(command_ack); +} + +int Commander::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 40, + 3250, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + // wait until task is up & running + if (wait_until_running() < 0) { + _task_id = -1; + return -1; + } + + return 0; +} + +Commander *Commander::instantiate(int argc, char *argv[]) +{ + Commander *instance = new Commander(); + + if (instance) { + if (argc >= 2 && !strcmp(argv[1], "-h")) { + instance->enable_hil(); + } + } + + return instance; +} + +void Commander::enable_hil() +{ + _status.hil_state = vehicle_status_s::HIL_STATE_ON; +} + +void Commander::mission_init() +{ + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ + mission_s mission; + + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { + if (mission.count > 0) { + PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs, curr: %" PRId32, mission.dataman_id, mission.count, + mission.current_seq); + } + + } else { + PX4_ERR("reading mission state failed"); + + /* initialize mission state in dataman */ + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + } + + _mission_pub.publish(mission); + } +} + +void Commander::data_link_check() +{ + for (auto &telemetry_status : _telemetry_status_subs) { + telemetry_status_s telemetry; + + if (telemetry_status.update(&telemetry)) { + + // handle different radio types + switch (telemetry.type) { + case telemetry_status_s::LINK_TYPE_USB: + // set (but don't unset) telemetry via USB as active once a MAVLink connection is up + _status_flags.usb_connected = true; + break; + + case telemetry_status_s::LINK_TYPE_IRIDIUM: { + iridiumsbd_status_s iridium_status; + + if (_iridiumsbd_status_sub.update(&iridium_status)) { + _high_latency_datalink_heartbeat = iridium_status.last_heartbeat; + + if (_status.high_latency_data_link_lost) { + if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) { + _status.high_latency_data_link_lost = false; + _status_changed = true; + } + } + + const bool present = true; + const bool enabled = true; + const bool ok = (iridium_status.last_heartbeat > 0); // maybe at some point here an additional check should be made + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SATCOM, present, enabled, ok, _status); + } + + break; + } + } + + if (telemetry.heartbeat_type_gcs) { + // Initial connection or recovery from data link lost + if (_status.data_link_lost) { + _status.data_link_lost = false; + _status_changed = true; + + if (_datalink_last_heartbeat_gcs != 0) { + mavlink_log_info(&_mavlink_log_pub, "Data link regained"); + } + + if (!_armed.armed && !_status_flags.condition_calibration_enabled) { + // make sure to report preflight check failures to a connecting GCS + PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, true, true, + hrt_elapsed_time(&_boot_timestamp)); + } + } + + _datalink_last_heartbeat_gcs = telemetry.timestamp; + } + + if (telemetry.heartbeat_type_onboard_controller) { + if (_onboard_controller_lost) { + _onboard_controller_lost = false; + _status_changed = true; + + if (_datalink_last_heartbeat_onboard_controller != 0) { + mavlink_log_info(&_mavlink_log_pub, "Onboard controller regained"); + } + } + + _datalink_last_heartbeat_onboard_controller = telemetry.timestamp; + } + + if (telemetry.heartbeat_component_obstacle_avoidance) { + if (_avoidance_system_lost) { + _avoidance_system_lost = false; + _status_changed = true; + } + + _datalink_last_heartbeat_avoidance_system = telemetry.timestamp; + _status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy; + } + } + } + + + // GCS data link loss failsafe + if (!_status.data_link_lost) { + if ((_datalink_last_heartbeat_gcs != 0) + && hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) { + + _status.data_link_lost = true; + _status.data_link_lost_counter++; + + mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost"); + + _status_changed = true; + } + } + + // ONBOARD CONTROLLER data link loss failsafe + if ((_datalink_last_heartbeat_onboard_controller > 0) + && (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > (_param_com_obc_loss_t.get() * 1_s)) + && !_onboard_controller_lost) { + + mavlink_log_critical(&_mavlink_log_pub, "Connection to mission computer lost"); + _onboard_controller_lost = true; + _status_changed = true; + } + + // AVOIDANCE SYSTEM state check (only if it is enabled) + if (_status_flags.avoidance_system_required && !_onboard_controller_lost) { + // if heartbeats stop + if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0) + && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) { + + _avoidance_system_lost = true; + _status_flags.avoidance_system_valid = false; + } + } + + // high latency data link loss failsafe + if (_high_latency_datalink_heartbeat > 0 + && hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) { + _high_latency_datalink_lost = hrt_absolute_time(); + + if (!_status.high_latency_data_link_lost) { + _status.high_latency_data_link_lost = true; + mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost"); + _status_changed = true; + } + } +} + +void Commander::avoidance_check() +{ + for (auto &dist_sens_sub : _distance_sensor_subs) { + distance_sensor_s distance_sensor; + + if (dist_sens_sub.update(&distance_sensor)) { + if ((distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { + + _valid_distance_sensor_time_us = distance_sensor.timestamp; + } + } + } + + const bool cp_enabled = _param_cp_dist.get() > 0.f; + + const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms; + const bool cp_healthy = _status_flags.avoidance_system_valid || distance_sensor_valid; + + const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || cp_enabled; + + const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled; + const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled); + + const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_enabled)); + const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy)); + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled, + sensor_oa_healthy, _status); +} + +void Commander::battery_status_check() +{ + // We need to update the status flag if ANY battery is updated, because the system source might have + // changed, or might be nothing (if there is no battery connected) + if (!_battery_status_subs.updated()) { + // Nothing has changed since the last time this function was called, so nothing needs to be done now. + return; + } + + battery_status_s batteries[_battery_status_subs.size()]; + size_t num_connected_batteries = 0; + + for (auto &battery_sub : _battery_status_subs) { + if (battery_sub.copy(&batteries[num_connected_batteries])) { + if (batteries[num_connected_batteries].connected) { + num_connected_batteries++; + } + } + } + + // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest + // option is to check if ANY of them have a warning, and specifically find which one has the most + // urgent warning. + uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; + // To make sure that all connected batteries are being regularly reported, we check which one has the + // oldest timestamp. + hrt_abstime oldest_update = hrt_absolute_time(); + + _battery_current = 0.0f; + float battery_level = 0.0f; + + + // Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing. + for (size_t i = 0; i < num_connected_batteries; i++) { + if (batteries[i].warning > worst_warning) { + worst_warning = batteries[i].warning; + } + + if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) { + oldest_update = batteries[i].timestamp; + } + + // Sum up current from all batteries. + _battery_current += batteries[i].current_filtered_a; + + // average levels from all batteries + battery_level += batteries[i].remaining; + } + + battery_level /= num_connected_batteries; + + _rtl_flight_time_sub.update(); + float battery_usage_to_home = 0; + + if (hrt_absolute_time() - _rtl_flight_time_sub.get().timestamp < 2_s) { + battery_usage_to_home = _rtl_flight_time_sub.get().rtl_limit_fraction; + } + + uint8_t battery_range_warning = battery_status_s::BATTERY_WARNING_NONE; + + if (PX4_ISFINITE(battery_usage_to_home)) { + float battery_at_home = battery_level - battery_usage_to_home; + + if (battery_at_home < _param_bat_crit_thr.get()) { + battery_range_warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else if (battery_at_home < _param_bat_low_thr.get()) { + battery_range_warning = battery_status_s::BATTERY_WARNING_LOW; + } + } + + if (battery_range_warning > worst_warning) { + worst_warning = battery_range_warning; + } + + bool battery_warning_level_increased_while_armed = false; + bool update_internal_battery_state = false; + + if (_armed.armed) { + if (worst_warning > _battery_warning) { + battery_warning_level_increased_while_armed = true; + update_internal_battery_state = true; + } + + } else { + if (_battery_warning != worst_warning) { + update_internal_battery_state = true; + } + } + + if (update_internal_battery_state) { + _battery_warning = worst_warning; + } + + + _status_flags.condition_battery_healthy = + // All connected batteries are regularly being published + (hrt_elapsed_time(&oldest_update) < 5_s) + // There is at least one connected battery (in any slot) + && (num_connected_batteries > 0) + // No currently-connected batteries have any warning + && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE); + + // execute battery failsafe if the state has gotten worse while we are armed + if (battery_warning_level_increased_while_armed) { + battery_failsafe(&_mavlink_log_pub, _status, _status_flags, _internal_state, _battery_warning, + (low_battery_action_t)_param_com_low_bat_act.get()); + } + + // Handle shutdown request from emergency battery action + if (update_internal_battery_state) { + + if (_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { +#if defined(CONFIG_BOARDCTL_POWEROFF) + + if (shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) { + mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down"); + + while (1) { px4_usleep(1); } + + } else { + mavlink_log_critical(&_mavlink_log_pub, "System does not support shutdown"); + } + +#endif // CONFIG_BOARDCTL_POWEROFF + } + } +} + +void Commander::estimator_check() +{ + // Check if quality checking of position accuracy and consistency is to be performed + const bool run_quality_checks = !_status_flags.circuit_breaker_engaged_posfailure_check; + + _local_position_sub.update(); + _global_position_sub.update(); + + const vehicle_local_position_s &lpos = _local_position_sub.get(); + + if (lpos.heading_reset_counter != _heading_reset_counter) { + if (_status_flags.condition_home_position_valid) { + updateHomePositionYaw(_home_pub.get().yaw + lpos.delta_heading); + } + + _heading_reset_counter = lpos.heading_reset_counter; + } + + const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); + + // 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); + } + } + } + + if (_estimator_status_sub.update()) { + const estimator_status_s &estimator_status = _estimator_status_sub.get(); + + // Check for a magnetomer fault and notify the user + const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); + + if (!mag_fault_prev && mag_fault) { + mavlink_log_critical(&_mavlink_log_pub, "Stopping compass use! Check calibration on landing"); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status); + } + + /* Check estimator status for signs of bad yaw induced post takeoff navigation failure + * for a short time interval after takeoff. + * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial + * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but + * if this does not fix the issue we need to stop using a position controlled + * mode to prevent flyaway crashes. + */ + + if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + _nav_test_failed = false; + _nav_test_passed = false; + + } else { + if (!_nav_test_passed) { + // Both test ratios need to pass/fail together to change the nav test status + const bool innovation_pass = (estimator_status.vel_test_ratio < 1.0f) && (estimator_status.pos_test_ratio < 1.0f) + && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON); + const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.0f) && (estimator_status.pos_test_ratio >= 1.0f); + + if (innovation_pass) { + _time_last_innov_pass = hrt_absolute_time(); + + // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed + const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s); + const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f); + + // Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds + if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s + && (sufficient_time || sufficient_speed)) { + _nav_test_passed = true; + _nav_test_failed = false; + } + + } else if (innovation_fail) { + _time_last_innov_fail = hrt_absolute_time(); + + if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) { + // if the innovation test has failed continuously, declare the nav as failed + _nav_test_failed = true; + mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors"); + } + } + } + } + } + } + + // run position and velocity accuracy checks + // Check if quality checking of position accuracy and consistency is to be performed + if (run_quality_checks) { + UpdateEstimateValidity(); + } + + _status_flags.condition_local_altitude_valid = lpos.z_valid + && (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s)); + + + // attitude + vehicle_attitude_s attitude{}; + _vehicle_attitude_sub.copy(&attitude); + const matrix::Quatf q{attitude.q}; + const matrix::Quatf q_norm{q.unit()}; + _status_flags.condition_attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s) + && (matrix::Quatf(q - q_norm).length() < FLT_EPSILON); + + // angular velocity + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&angular_velocity); + _status_flags.condition_angular_velocity_valid = (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s) + && PX4_ISFINITE(angular_velocity.xyz[0]) && PX4_ISFINITE(angular_velocity.xyz[1]) + && PX4_ISFINITE(angular_velocity.xyz[2]); +} + +void Commander::UpdateEstimateValidity() +{ + const vehicle_local_position_s &lpos = _local_position_sub.get(); + const vehicle_global_position_s &gpos = _global_position_sub.get(); + const estimator_status_s &status = _estimator_status_sub.get(); + + float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get(); + + // relax local position eph threshold in operator controlled position mode + if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL && + ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) + || (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) { + + // Set the allowable position uncertainty based on combination of flight and estimator state + // When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift + const bool reliant_on_opt_flow = ((status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW)) + && !(status.control_mode_flags & (1 << estimator_status_s::CS_GPS)) + && !(status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS))); + + if (reliant_on_opt_flow) { + lpos_eph_threshold_adj = INFINITY; + } + } + + _status_flags.condition_global_position_valid = + check_posvel_validity(lpos.xy_valid && !_nav_test_failed, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, + &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid); + + _status_flags.condition_local_position_valid = + check_posvel_validity(lpos.xy_valid && !_nav_test_failed, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp, + &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid); + + _status_flags.condition_local_velocity_valid = + check_posvel_validity(lpos.v_xy_valid && !_nav_test_failed, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, + &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid); +} + +void +Commander::offboard_control_update() +{ + bool offboard_available = false; + + if (_offboard_control_mode_sub.updated()) { + const offboard_control_mode_s old = _offboard_control_mode_sub.get(); + + if (_offboard_control_mode_sub.update()) { + const offboard_control_mode_s &ocm = _offboard_control_mode_sub.get(); + + if (old.position != ocm.position || + old.velocity != ocm.velocity || + old.acceleration != ocm.acceleration || + old.attitude != ocm.attitude || + old.body_rate != ocm.body_rate) { + + _status_changed = true; + } + + if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate) { + offboard_available = true; + } + } + } + + if (_offboard_control_mode_sub.get().position && !_status_flags.condition_local_position_valid) { + offboard_available = false; + + } else if (_offboard_control_mode_sub.get().velocity && !_status_flags.condition_local_velocity_valid) { + offboard_available = false; + + } else if (_offboard_control_mode_sub.get().acceleration && !_status_flags.condition_local_velocity_valid) { + // OFFBOARD acceleration handled by position controller + offboard_available = false; + } + + _offboard_available.set_state_and_update(offboard_available, hrt_absolute_time()); + + const bool offboard_lost = !_offboard_available.get_state(); + + if (_status_flags.offboard_control_signal_lost != offboard_lost) { + _status_flags.offboard_control_signal_lost = offboard_lost; + _status_changed = true; + } +} + +void Commander::esc_status_check() +{ + esc_status_s esc_status{}; + + _esc_status_sub.copy(&esc_status); + + if (esc_status.esc_count > 0) { + + char esc_fail_msg[50]; + esc_fail_msg[0] = '\0'; + + int online_bitmask = (1 << esc_status.esc_count) - 1; + + // Check if ALL the ESCs are online + if (online_bitmask == esc_status.esc_online_flags) { + + _status_flags.condition_escs_error = false; + _last_esc_online_flags = esc_status.esc_online_flags; + + } else if (_last_esc_online_flags == esc_status.esc_online_flags) { + + // Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver + + _status_flags.condition_escs_error = true; + + } else if (esc_status.esc_online_flags < _last_esc_online_flags) { + + // Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot + + for (int index = 0; index < esc_status.esc_count; index++) { + if ((esc_status.esc_online_flags & (1 << index)) == 0) { + snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1); + esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; + } + } + + mavlink_log_critical(&_mavlink_log_pub, "%soffline", esc_fail_msg); + + _last_esc_online_flags = esc_status.esc_online_flags; + _status_flags.condition_escs_error = true; + } + + _status_flags.condition_escs_failure = false; + + for (int index = 0; index < esc_status.esc_count; index++) { + + if (esc_status.esc[index].failures > esc_report_s::FAILURE_NONE) { + _status_flags.condition_escs_failure = true; + + if (esc_status.esc[index].failures != _last_esc_failure[index]) { + + if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_CURRENT_MASK) { + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over current", index + 1); + } + + if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_VOLTAGE_MASK) { + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over voltage", index + 1); + } + + if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_TEMPERATURE_MASK) { + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over temperature", index + 1); + } + + if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_RPM_MASK) { + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over RPM", index + 1); + } + + if (esc_status.esc[index].failures & esc_report_s::FAILURE_INCONSISTENT_CMD_MASK) { + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: command inconsistency", index + 1); + } + + if (esc_status.esc[index].failures & esc_report_s::FAILURE_MOTOR_STUCK_MASK) { + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: motor stuck", index + 1); + } + + if (esc_status.esc[index].failures & esc_report_s::FAILURE_GENERIC_MASK) { + mavlink_log_critical(&_mavlink_log_pub, "ESC%d: generic failure - code %d", index + 1, esc_status.esc[index].esc_state); + } + + } + + _last_esc_failure[index] = esc_status.esc[index].failures; + } + } + + } + + _last_esc_status_updated = esc_status.timestamp; +} + +void Commander::send_parachute_command() +{ + vehicle_command_s vcmd{}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE; + vcmd.param1 = static_cast(vehicle_command_s::PARACHUTE_ACTION_RELEASE); + + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + vcmd.source_system = vehicle_status_sub.get().system_id; + vcmd.target_system = vehicle_status_sub.get().system_id; + vcmd.source_component = vehicle_status_sub.get().component_id; + vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE + + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; + vcmd.timestamp = hrt_absolute_time(); + vcmd_pub.publish(vcmd); + + set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); +} + +int Commander::print_usage(const char *reason) +{ + if (reason) { + PX4_INFO("%s", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The commander module contains the state machine for mode switching and failsafe behavior. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("commander", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('h', "Enable HIL mode", true); +#ifndef CONSTRAINED_FLASH + PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration"); + PRINT_MODULE_USAGE_ARG("mag|accel|gyro|level|esc|airspeed", "Calibration type", false); + PRINT_MODULE_USAGE_ARG("quick", "Quick calibration (accel only, not recommended)", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks"); + PRINT_MODULE_USAGE_COMMAND("arm"); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true); + PRINT_MODULE_USAGE_COMMAND("disarm"); + PRINT_MODULE_USAGE_COMMAND("takeoff"); + PRINT_MODULE_USAGE_COMMAND("land"); + PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); + PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland", + "Flight mode", false); + PRINT_MODULE_USAGE_COMMAND("pair"); + PRINT_MODULE_USAGE_COMMAND("lockdown"); + PRINT_MODULE_USAGE_ARG("off", "Turn lockdown off", true); + PRINT_MODULE_USAGE_COMMAND("set_ekf_origin"); + PRINT_MODULE_USAGE_ARG("lat, lon, alt", "Origin Latitude, Longitude, Altitude", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("lat|lon|alt", "Origin latitude longitude altitude"); +#endif + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 1; +} diff --git a/src/prometheus_px4/src/modules/commander/Commander.hpp b/src/prometheus_px4/src/modules/commander/Commander.hpp new file mode 100644 index 0000000..0c14b2f --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/Commander.hpp @@ -0,0 +1,444 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +// publications +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// subscriptions +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using math::constrain; +using systemlib::Hysteresis; + +using namespace time_literals; + +class Commander : public ModuleBase, 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) _param_nav_dll_act, + (ParamInt) _param_com_dl_loss_t, + + (ParamInt) _param_com_hldl_loss_t, + (ParamInt) _param_com_hldl_reg_t, + + (ParamInt) _param_nav_rcl_act, + (ParamFloat) _param_com_rcl_act_t, + (ParamInt) _param_com_rcl_except, + + (ParamFloat) _param_com_home_h_t, + (ParamFloat) _param_com_home_v_t, + (ParamBool) _param_com_home_in_air, + + (ParamFloat) _param_com_pos_fs_eph, + (ParamFloat) _param_com_pos_fs_epv, /*Not realy used for now*/ + (ParamFloat) _param_com_vel_fs_evh, + (ParamInt) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */ + + (ParamInt) _param_com_pos_fs_delay, + (ParamInt) _param_com_pos_fs_prob, + (ParamInt) _param_com_pos_fs_gain, + + (ParamInt) _param_com_low_bat_act, + (ParamFloat) _param_com_disarm_land, + (ParamFloat) _param_com_disarm_preflight, + + (ParamBool) _param_com_obs_avoid, + + (ParamInt) _param_com_flt_profile, + + (ParamFloat) _param_com_obc_loss_t, + + // Offboard + (ParamFloat) _param_com_of_loss_t, + (ParamInt) _param_com_obl_act, + (ParamInt) _param_com_obl_rc_act, + + (ParamInt) _param_com_prearm_mode, + (ParamBool) _param_com_mot_test_en, + + (ParamFloat) _param_com_kill_disarm, + (ParamFloat) _param_com_lkdown_tko, + + // Engine failure + (ParamFloat) _param_ef_throttle_thres, + (ParamFloat) _param_ef_current2throttle_thres, + (ParamFloat) _param_ef_time_thres, + + (ParamBool) _param_arm_without_gps, + (ParamBool) _param_arm_mission_required, + (ParamBool) _param_arm_auth_required, + (ParamBool) _param_escs_checks_required, + (ParamBool) _param_com_rearm_grace, + + (ParamInt) _param_flight_uuid, + (ParamInt) _param_takeoff_finished_action, + + (ParamInt) _param_rc_in_off, + + (ParamInt) _param_fltmode_1, + (ParamInt) _param_fltmode_2, + (ParamInt) _param_fltmode_3, + (ParamInt) _param_fltmode_4, + (ParamInt) _param_fltmode_5, + (ParamInt) _param_fltmode_6, + + // Circuit breakers + (ParamInt) _param_cbrk_supply_chk, + (ParamInt) _param_cbrk_usb_chk, + (ParamInt) _param_cbrk_airspd_chk, + (ParamInt) _param_cbrk_enginefail, + (ParamInt) _param_cbrk_flightterm, + (ParamInt) _param_cbrk_velposerr, + (ParamInt) _param_cbrk_vtolarming, + + // Geofrence + (ParamInt) _param_geofence_action, + + // Mavlink + (ParamInt) _param_mav_comp_id, + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_type, + + (ParamFloat) _param_cp_dist, + + (ParamFloat) _param_bat_low_thr, + (ParamFloat) _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_subs{ORB_ID::battery_status}; + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + uORB::SubscriptionMultiArray _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_sub{ORB_ID(airspeed)}; + uORB::SubscriptionData _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; + uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData _rtl_flight_time_sub{ORB_ID(rtl_flight_time)}; + + // Publications + uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; + uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; + uORB::Publication _test_motor_pub{ORB_ID(test_motor)}; + uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; + uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; + uORB::Publication _status_pub{ORB_ID(vehicle_status)}; + uORB::Publication _mission_pub{ORB_ID(mission)}; + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + + uORB::PublicationData _home_pub{ORB_ID(home_position)}; + + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + orb_advert_t _mavlink_log_pub{nullptr}; +}; diff --git a/src/prometheus_px4/src/modules/commander/ManualControl.cpp b/src/prometheus_px4/src/modules/commander/ManualControl.cpp new file mode 100644 index 0000000..6d9bc6b --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/ManualControl.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * 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 + +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); +} diff --git a/src/prometheus_px4/src/modules/commander/ManualControl.hpp b/src/prometheus_px4/src/modules/commander/ManualControl.hpp new file mode 100644 index 0000000..862b2af --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/ManualControl.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +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) _param_com_rc_loss_t, + (ParamInt) _param_rc_arm_hyst, + (ParamBool) _param_com_arm_swisbtn, + (ParamBool) _param_com_rearm_grace, + (ParamInt) _param_rc_override, + (ParamFloat) _param_com_rc_stick_ov + ) +}; diff --git a/src/prometheus_px4/src/modules/commander/accelerometer_calibration.cpp b/src/prometheus_px4/src/modules/commander/accelerometer_calibration.cpp new file mode 100644 index 0000000..763470d --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/accelerometer_calibration.cpp @@ -0,0 +1,605 @@ +/**************************************************************************** + * + * 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 + */ + +#include "accelerometer_calibration.h" +#include "calibration_messages.h" +#include "calibration_routines.h" +#include "commander_helper.h" +#include "factory_calibration_storage.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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 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 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 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; +} diff --git a/src/prometheus_px4/src/modules/commander/accelerometer_calibration.h b/src/prometheus_px4/src/modules/commander/accelerometer_calibration.h new file mode 100644 index 0000000..3eeb961 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/accelerometer_calibration.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 + */ + +#ifndef ACCELEROMETER_CALIBRATION_H_ +#define ACCELEROMETER_CALIBRATION_H_ + +#include +#include + +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_ */ diff --git a/src/prometheus_px4/src/modules/commander/airspeed_calibration.cpp b/src/prometheus_px4/src/modules/commander/airspeed_calibration.cpp new file mode 100644 index 0000000..25b58b2 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/airspeed_calibration.cpp @@ -0,0 +1,292 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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; +} diff --git a/src/prometheus_px4/src/modules/commander/airspeed_calibration.h b/src/prometheus_px4/src/modules/commander/airspeed_calibration.h new file mode 100644 index 0000000..3a86b00 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/airspeed_calibration.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 +#include + +int do_airspeed_calibration(orb_advert_t *mavlink_log_pub); + +#endif /* AIRSPEED_CALIBRATION_H_ */ diff --git a/src/prometheus_px4/src/modules/commander/calibration_messages.h b/src/prometheus_px4/src/modules/commander/calibration_messages.h new file mode 100644 index 0000000..8afcf79 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/calibration_messages.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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 + */ + +#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_ */ diff --git a/src/prometheus_px4/src/modules/commander/calibration_routines.cpp b/src/prometheus_px4/src/modules/commander/calibration_routines.cpp new file mode 100644 index 0000000..a5e645e --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/calibration_routines.cpp @@ -0,0 +1,367 @@ +/**************************************************************************** + * + * 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 + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#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_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 command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); + + return ret; + } + } + } + + return ret; +} diff --git a/src/prometheus_px4/src/modules/commander/calibration_routines.h b/src/prometheus_px4/src/modules/commander/calibration_routines.h new file mode 100644 index 0000000..a18fa1d --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/calibration_routines.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 + +#pragma once + +#include +#include + +// 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); diff --git a/src/prometheus_px4/src/modules/commander/commander_helper.cpp b/src/prometheus_px4/src/modules/commander/commander_helper.cpp new file mode 100644 index 0000000..dfff253 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/commander_helper.cpp @@ -0,0 +1,371 @@ +/**************************************************************************** + * + * 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 + * @author Julian Oes + * @author Anton Babushkin + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 ¤t_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 ¤t_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 ¤t_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 ¤t_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 ¤t_status) +{ + return current_status.system_type == VEHICLE_TYPE_FIXED_WING; +} + +bool is_ground_rover(const vehicle_status_s ¤t_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); +} diff --git a/src/prometheus_px4/src/modules/commander/commander_helper.h b/src/prometheus_px4/src/modules/commander/commander_helper.h new file mode 100644 index 0000000..8ff0922 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/commander_helper.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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 + * @author Julian Oes + */ + +#ifndef COMMANDER_HELPER_H_ +#define COMMANDER_HELPER_H_ + +#include +#include +#include +#include +#include +#include + + +bool is_multirotor(const vehicle_status_s ¤t_status); +bool is_rotary_wing(const vehicle_status_s ¤t_status); +bool is_vtol(const vehicle_status_s ¤t_status); +bool is_vtol_tailsitter(const vehicle_status_s ¤t_status); +bool is_fixed_wing(const vehicle_status_s ¤t_status); +bool is_ground_rover(const vehicle_status_s ¤t_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_ */ diff --git a/src/prometheus_px4/src/modules/commander/commander_params.c b/src/prometheus_px4/src/modules/commander/commander_params.c new file mode 100644 index 0000000..ebc0589 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/commander_params.c @@ -0,0 +1,1029 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 commander_params.c + * + * Parameters defined by the sensors task. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + */ + +/** + * Roll trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); + +/** + * Pitch trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); + +/** + * Yaw trim + * + * The trim value is the actuator control value the system needs + * for straight and level flight. It can be calibrated by + * flying manually straight and level using the RC trims and + * copying them using the GCS. + * + * @group Radio Calibration + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +/** + * Datalink loss time threshold + * + * After this amount of seconds without datalink the data link lost mode triggers + * + * @group Commander + * @unit s + * @min 5 + * @max 300 + * @decimal 1 + * @increment 1 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); + +/** + * High Latency Datalink loss time threshold + * + * After this amount of seconds without datalink the data link lost mode triggers + * + * @group Commander + * @unit s + * @min 60 + * @max 3600 + */ +PARAM_DEFINE_INT32(COM_HLDL_LOSS_T, 120); + +/** + * High Latency Datalink regain time threshold + * + * After a data link loss: after this number of seconds with a healthy datalink the 'datalink loss' + * flag is set back to false + * + * @group Commander + * @unit s + * @min 0 + * @max 60 + */ +PARAM_DEFINE_INT32(COM_HLDL_REG_T, 0); + +/** + * Engine Failure Throttle Threshold + * + * Engine failure triggers only above this throttle value + * + * @group Commander + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); + +/** + * Engine Failure Current/Throttle Threshold + * + * Engine failure triggers only below this current value + * + * @group Commander + * @min 0.0 + * @max 50.0 + * @unit A/% + * @decimal 2 + * @increment 1 + */ +PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); + +/** + * Engine Failure Time Threshold + * + * Engine failure triggers only if the throttle threshold and the + * current to throttle threshold are violated for this time + * + * @group Commander + * @unit s + * @min 0.0 + * @max 60.0 + * @decimal 1 + * @increment 1 + */ +PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); + +/** + * RC loss time threshold + * + * After this amount of seconds without RC connection it's considered lost and not used anymore + * + * @group Commander + * @unit s + * @min 0 + * @max 35 + * @decimal 1 + * @increment 0.1 + */ +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); + +/** + * Delay between RC loss and configured reaction + * + * RC signal not updated -> still use data for COM_RC_LOSS_T seconds + * Consider RC signal lost -> wait COM_RCL_ACT_T seconds on the spot waiting to regain signal + * React with failsafe action NAV_RCL_ACT + * + * A zero value disables the delay. + * + * @group Commander + * @unit s + * @min 0.0 + * @max 25.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(COM_RCL_ACT_T, 15.0f); + +/** + * Home set horizontal threshold + * + * The home position will be set if the estimated positioning accuracy is below the threshold. + * + * @group Commander + * @unit m + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.5 + */ +PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); + +/** + * Home set vertical threshold + * + * The home position will be set if the estimated positioning accuracy is below the threshold. + * + * @group Commander + * @unit m + * @min 5 + * @max 25 + * @decimal 2 + * @increment 0.5 + */ +PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f); + +/** + * Allows setting the home position after takeoff + * + * If set to true, the autopilot is allowed to set its home position after takeoff + * The true home position is back-computed if a local position is estimate if available. + * If no local position is available, home is set to the current position. + * + * @boolean + * @group Commander + */ +PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0); + +/** + * RC control input mode + * + * The default value of 0 requires a valid RC transmitter setup. + * Setting this to 1 allows joystick control and disables RC input handling and the associated checks. A value of + * 2 will generate RC control data from manual input received via MAVLink instead + * of directly forwarding the manual input data. + * + * @group Commander + * @min 0 + * @max 2 + * @value 0 RC Transmitter + * @value 1 Joystick/No RC Checks + * @value 2 Virtual RC by Joystick + */ +PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); + +/** + * RC input arm/disarm command duration + * + * The default value of 1000 requires the stick to be held in the arm or disarm position for 1 second. + * + * @group Commander + * @min 100 + * @max 1500 + * @unit ms + */ +PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000); + +/** + * Time-out for auto disarm after landing + * + * A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be + * automatically disarmed in case a landing situation has been detected during this period. + * + * A zero or negative value means that automatic disarming triggered by landing detection is disabled. + * + * @group Commander + * @unit s + * @decimal 2 + */ + +PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f); + +/** + * Time-out for auto disarm if not taking off + * + * A non-zero, positive value specifies the time in seconds, within which the + * vehicle is expected to take off after arming. In case the vehicle didn't takeoff + * within the timeout it disarms again. + * + * A negative value disables autmoatic disarming triggered by a pre-takeoff timeout. + * + * @group Commander + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f); + +/** + * Allow arming without GPS + * + * The default allows the vehicle to arm without GPS signal. + * + * @group Commander + * @value 0 Require GPS lock to arm + * @value 1 Allow arming without GPS + */ +PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1); + +/** + * Arm switch is a momentary button + * + * 0: Arming/disarming triggers on switch transition. + * 1: Arming/disarming triggers when holding the momentary button down + * for COM_RC_ARM_HYST like the stick gesture. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0); + +/** + * Battery failsafe mode + * + * Action the system takes at critical battery. See also BAT_CRIT_THR and BAT_EMERGEN_THR + * for definition of battery states. + * + * @group Commander + * @value 0 Warning + * @value 2 Land mode + * @value 3 Return at critical level, land at emergency level + * @decimal 0 + * @increment 1 + */ +PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); + +/** + * Time-out to wait when offboard connection is lost before triggering offboard lost action. + * + * See COM_OBL_ACT and COM_OBL_RC_ACT to configure action. + * + * @group Commander + * @unit s + * @min 0 + * @max 60 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f); + +/** + * Set offboard loss failsafe mode + * + * The offboard loss failsafe will only be entered after a timeout, + * set by COM_OF_LOSS_T in seconds. + * + * @value -1 Disabled + * @value 0 Land mode + * @value 1 Hold mode + * @value 2 Return mode + * @value 3 Terminate + * @value 4 Lockdown + * + * @group Commander + */ +PARAM_DEFINE_INT32(COM_OBL_ACT, 0); + +/** + * Set offboard loss failsafe mode when RC is available + * + * The offboard loss failsafe will only be entered after a timeout, + * set by COM_OF_LOSS_T in seconds. + * + * @value -1 Disabled + * @value 0 Position mode + * @value 1 Altitude mode + * @value 2 Manual + * @value 3 Return mode + * @value 4 Land mode + * @value 5 Hold mode + * @value 6 Terminate + * @value 7 Lockdown + * @group Commander + */ +PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); + +/** + * Time-out to wait when onboard computer connection is lost before warning about loss connection. + * + * @group Commander + * @unit s + * @min 0 + * @max 60 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f); + +/** + * First flightmode slot (1000-1160) + * + * If the main switch channel is in this range the + * selected flight mode will be applied. + * + * @value -1 Unassigned + * @value 0 Manual + * @value 1 Altitude + * @value 2 Position + * @value 3 Mission + * @value 4 Hold + * @value 10 Takeoff + * @value 11 Land + * @value 5 Return + * @value 6 Acro + * @value 7 Offboard + * @value 8 Stabilized + * @value 12 Follow Me + * @group Commander + */ +PARAM_DEFINE_INT32(COM_FLTMODE1, -1); + +/** + * Second flightmode slot (1160-1320) + * + * If the main switch channel is in this range the + * selected flight mode will be applied. + * + * @value -1 Unassigned + * @value 0 Manual + * @value 1 Altitude + * @value 2 Position + * @value 3 Mission + * @value 4 Hold + * @value 10 Takeoff + * @value 11 Land + * @value 5 Return + * @value 6 Acro + * @value 7 Offboard + * @value 8 Stabilized + * @value 12 Follow Me + * @group Commander + */ +PARAM_DEFINE_INT32(COM_FLTMODE2, -1); + +/** + * Third flightmode slot (1320-1480) + * + * If the main switch channel is in this range the + * selected flight mode will be applied. + * + * @value -1 Unassigned + * @value 0 Manual + * @value 1 Altitude + * @value 2 Position + * @value 3 Mission + * @value 4 Hold + * @value 10 Takeoff + * @value 11 Land + * @value 5 Return + * @value 6 Acro + * @value 7 Offboard + * @value 8 Stabilized + * @value 12 Follow Me + * @group Commander + */ +PARAM_DEFINE_INT32(COM_FLTMODE3, -1); + +/** + * Fourth flightmode slot (1480-1640) + * + * If the main switch channel is in this range the + * selected flight mode will be applied. + * + * @value -1 Unassigned + * @value 0 Manual + * @value 1 Altitude + * @value 2 Position + * @value 3 Mission + * @value 4 Hold + * @value 10 Takeoff + * @value 11 Land + * @value 5 Return + * @value 6 Acro + * @value 7 Offboard + * @value 8 Stabilized + * @value 12 Follow Me + * @group Commander + */ +PARAM_DEFINE_INT32(COM_FLTMODE4, -1); + +/** + * Fifth flightmode slot (1640-1800) + * + * If the main switch channel is in this range the + * selected flight mode will be applied. + * + * @value -1 Unassigned + * @value 0 Manual + * @value 1 Altitude + * @value 2 Position + * @value 3 Mission + * @value 4 Hold + * @value 10 Takeoff + * @value 11 Land + * @value 5 Return + * @value 6 Acro + * @value 7 Offboard + * @value 8 Stabilized + * @value 12 Follow Me + * @group Commander + */ +PARAM_DEFINE_INT32(COM_FLTMODE5, -1); + +/** + * Sixth flightmode slot (1800-2000) + * + * If the main switch channel is in this range the + * selected flight mode will be applied. + * + * @value -1 Unassigned + * @value 0 Manual + * @value 1 Altitude + * @value 2 Position + * @value 3 Mission + * @value 4 Hold + * @value 10 Takeoff + * @value 11 Land + * @value 5 Return + * @value 6 Acro + * @value 7 Offboard + * @value 8 Stabilized + * @value 12 Follow Me + * @group Commander + */ +PARAM_DEFINE_INT32(COM_FLTMODE6, -1); + +/** + * Maximum EKF position innovation test ratio that will allow arming + * + * @group Commander + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_POS, 0.5f); + +/** + * Maximum EKF velocity innovation test ratio that will allow arming + * + * @group Commander + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_VEL, 0.5f); + +/** + * Maximum EKF height innovation test ratio that will allow arming + * + * @group Commander + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_HGT, 1.0f); + +/** + * Maximum EKF yaw innovation test ratio that will allow arming + * + * @group Commander + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f); + +/** + * Maximum accelerometer inconsistency between IMU units that will allow arming + * + * @group Commander + * @unit m/s^2 + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_ARM_IMU_ACC, 0.7f); + +/** + * Maximum rate gyro inconsistency between IMU units that will allow arming + * + * @group Commander + * @unit rad/s + * @min 0.02 + * @max 0.3 + * @decimal 3 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f); + +/** + * Maximum magnetic field inconsistency between units that will allow arming + * + * Set -1 to disable the check. + * + * @group Commander + * @unit deg + * @min 3 + * @max 180 + */ +PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45); + +/** + * Enable mag strength preflight check + * + * Deny arming if the estimator detects a strong magnetic + * disturbance (check enabled by EKF2_MAG_CHECK) + * + * @boolean + * @group Commander + */ +PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1); + +/** + * Rearming grace period + * + * Re-arming grace allows to rearm the drone with manual command without running prearmcheck during 5 s after disarming. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_REARM_GRACE, 1); + +/** + * Enable RC stick override of auto and/or offboard modes + * + * When RC stick override is enabled, moving the RC sticks more than COM_RC_STICK_OV from + * their center position immediately gives control back to the pilot by switching to Position mode. + * Note: Only has an effect on multicopters, and VTOLs in multicopter mode. + * + * @min 0 + * @max 7 + * @bit 0 Enable override during auto modes (except for in critical battery reaction) + * @bit 1 Enable override during offboard mode + * @bit 2 Ignore throttle stick + * @group Commander + */ +PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1); + +/** + * RC stick override threshold + * + * If COM_RC_OVERRIDE is enabled and the joystick input is moved more than this threshold + * the autopilot the pilot takes over control. + * + * @group Commander + * @unit % + * @min 5 + * @max 80 + * @decimal 0 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f); + +/** + * Require valid mission to arm + * + * The default allows to arm the vehicle without a valid mission. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); + +/** + * Position control navigation loss response. + * + * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control. + * Navigation accuracy checks can be disabled using the CBRK_VELPOSERR parameter, but doing so will remove protection for all flight modes. + * + * @value 0 Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. + * @value 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. + * + * @group Commander + */ +PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0); + +/** + * Require arm authorization to arm + * + * By default off. The default allows to arm the vehicle without a arm authorization. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_AUTH_REQ, 0); + +/** + * Arm authorizer system id + * + * Used if arm authorization is requested by COM_ARM_AUTH_REQ. + * + * @group Commander + */ +PARAM_DEFINE_INT32(COM_ARM_AUTH_ID, 10); + +/** + * Arm authorization method + * + * Methods: + * - one arm: request authorization and arm when authorization is received + * - two step arm: 1st arm command request an authorization and + * 2nd arm command arm the drone if authorized + * + * Used if arm authorization is requested by COM_ARM_AUTH_REQ. + * + * @group Commander + * @value 0 one arm + * @value 1 two step arm + */ +PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0); + +/** + * Arm authorization timeout + * + * Timeout for authorizer answer. + * Used if arm authorization is requested by COM_ARM_AUTH_REQ. + * + * @group Commander + * @unit s + * @decimal 1 + * @increment 0.1 + */ +PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1); + +/** + * Loss of position failsafe activation delay. + * + * This sets number of seconds that the position checks need to be failed before the failsafe will activate. + * The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5 and 10 should be used. + * + * @unit s + * @reboot_required true + * @group Commander + * @min 1 + * @max 100 + */ +PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1); + +/** + * Loss of position probation delay at takeoff. + * + * The probation delay is the number of seconds that the EKF innovation checks need to pass for the position to be declared good after it has been declared bad. + * The probation delay will be reset to this parameter value when takeoff is detected. + * After takeoff, if position checks are passing, the probation delay will reduce by one second for every lapsed second of valid position down to a minimum of 1 second. + * If position checks are failing, the probation delay will increase by COM_POS_FS_GAIN seconds for every lapsed second up to a maximum of 100 seconds. + * The default value has been optimised for rotary wing applications. For fixed wing applications, a value of 1 should be used. + * + * @unit s + * @reboot_required true + * @group Commander + * @min 1 + * @max 100 + */ +PARAM_DEFINE_INT32(COM_POS_FS_PROB, 30); + +/** + * Loss of position probation gain factor. + * + * This sets the rate that the loss of position probation time grows when position checks are failing. + * The default value has been optimised for rotary wing applications. For fixed wing applications a value of 0 should be used. + * + * @reboot_required true + * @group Commander + */ +PARAM_DEFINE_INT32(COM_POS_FS_GAIN, 10); + +/** + * Horizontal position error threshold. + * + * This is the horizontal position error (EPH) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing. + * + * @unit m + * @group Commander + */ +PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5); + +/** + * Vertical position error threshold. + * + * This is the vertical position error (EPV) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing. + * + * @unit m + * @group Commander + */ +PARAM_DEFINE_FLOAT(COM_POS_FS_EPV, 10); + +/** + * Horizontal velocity error threshold. + * + * This is the horizontal velocity error (EVH) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing. + * + * @unit m/s + * @group Commander + */ +PARAM_DEFINE_FLOAT(COM_VEL_FS_EVH, 1); + +/** + * Next flight UUID + * + * This number is incremented automatically after every flight on + * disarming in order to remember the next flight UUID. + * The first flight is 0. + * + * @group Commander + * @category system + * @volatile + * @min 0 + */ +PARAM_DEFINE_INT32(COM_FLIGHT_UUID, 0); + +/** + * Action after TAKEOFF has been accepted. + * + * The mode transition after TAKEOFF has completed successfully. + * + * @value 0 Hold + * @value 1 Mission (if valid) + * @group Commander + */ +PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0); + +/** + * Set data link loss failsafe mode + * + * The data link loss failsafe will only be entered after a timeout, + * set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected + * action will be executed. + * + * @value 0 Disabled + * @value 1 Hold mode + * @value 2 Return mode + * @value 3 Land mode + * @value 5 Terminate + * @value 6 Lockdown + * @min 0 + * @max 6 + * + * @group Commander + */ +PARAM_DEFINE_INT32(NAV_DLL_ACT, 0); + +/** + * Set RC loss failsafe mode + * + * The RC loss failsafe will only be entered after a timeout, + * set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled + * by setting the COM_RC_IN_MODE param it will not be triggered. + * + * @value 1 Hold mode + * @value 2 Return mode + * @value 3 Land mode + * @value 5 Terminate + * @value 6 Lockdown + * @min 1 + * @max 6 + * + * @group Commander + */ +PARAM_DEFINE_INT32(NAV_RCL_ACT, 2); + +/** + * RC loss exceptions + * + * Specify modes in which RC loss is ignored and the failsafe action not triggered. + * + * @min 0 + * @max 31 + * @bit 0 Mission + * @bit 1 Hold + * @bit 2 Offboard + * @group Commander + */ +PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); + +/** + * Flag to enable obstacle avoidance. + * + * @boolean + * @group Commander + */ +PARAM_DEFINE_INT32(COM_OBS_AVOID, 0); + +/** + * User Flight Profile + * + * Describes the intended use of the vehicle. + * Can be used by ground control software or log post processing. + * This param does not influence the behavior within the firmware. This means for example the control logic is independent of the setting of this param (but depends on other params). + * + * @value 0 Default + * @value 100 Pro User + * @value 200 Flight Tester + * @value 300 Developer + * + * @group Commander + */ +PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0); + +/** + * Enable checks on ESCs that report telemetry. + * + * If this parameter is set, the system will check ESC's online status and failures. + * This param is specific for ESCs reporting status. It shall be used only if ESCs support telemetry. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 0); + +/** + * Condition to enter prearmed mode + * + * Condition to enter the prearmed state, an intermediate state between disarmed and armed + * in which non-throttling actuators are active. + * + * @value 0 Disabled + * @value 1 Safety button + * @value 2 Always + * + * @group Commander + */ +PARAM_DEFINE_INT32(COM_PREARM_MODE, 0); + +/** + * Enable Motor Testing + * + * If set, enables the motor test interface via MAVLink (DO_MOTOR_TEST), that + * allows spinning the motors for testing purposes. + * + * @boolean + * @group Commander + */ +PARAM_DEFINE_INT32(COM_MOT_TEST_EN, 1); + +/** + * Timeout value for disarming when kill switch is engaged + * + * @group Commander + * @unit s + * @min 0.0 + * @max 30.0 + * @increment 0.1 + */ +PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f); + +/** + * Maximum allowed CPU load to still arm + * + * A negative value disables the check. + * + * @group Commander + * @unit % + * @min -1 + * @max 100 + * @increment 1 + */ +PARAM_DEFINE_FLOAT(COM_CPU_MAX, 90.0f); + +/** + * Required number of redundant power modules + * + * This configures a check to verify the expected number of 5V rail power supplies are present. By default only one is expected. + * Note: CBRK_SUPPLY_CHK disables all power checks including this one. + * + * @group Commander + * @min 0 + * @max 4 + */ +PARAM_DEFINE_INT32(COM_POWER_COUNT, 1); + +/** + * Timeout for detecting a failure after takeoff + * + * A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to put the vehicle into + * a lockdown state if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R. + * The check is not executed for flight modes that do support acrobatic maneuvers, e.g: Acro (MC/FW) and Manual (FW). + * A zero or negative value means that the check is disabled. + * + * @group Commander + * @unit s + * @min -1.0 + * @max 5.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f); + +/** +* Enable preflight check for maximal allowed airspeed when arming. +* +* Deny arming if the current airspeed measurement is greater than half the cruise airspeed (FW_AIRSPD_TRIM). +* Excessive airspeed measurements on ground are either caused by wind or bad airspeed calibration. +* +* @group Commander +* @value 0 Disabled +* @value 1 Enabled +*/ +PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1); + +/** + * Enable FMU SD card detection check + * + * This check detects if the FMU SD card is missing. + * Depending on the value of the parameter, the check can be + * disabled, warn only or deny arming. + * + * @group Commander + * @value 0 Disabled + * @value 1 Warning only + * @value 2 Enforce SD card presence + */ +PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1); diff --git a/src/prometheus_px4/src/modules/commander/commander_tests/CMakeLists.txt b/src/prometheus_px4/src/modules/commander/commander_tests/CMakeLists.txt new file mode 100644 index 0000000..64df7b6 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/commander_tests/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 + ) diff --git a/src/prometheus_px4/src/modules/commander/commander_tests/commander_tests.cpp b/src/prometheus_px4/src/modules/commander/commander_tests/commander_tests.cpp new file mode 100644 index 0000000..2bfa5d0 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/commander_tests/commander_tests.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 + +#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; +} diff --git a/src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.cpp new file mode 100644 index 0000000..177c654 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -0,0 +1,546 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 +#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) diff --git a/src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.h new file mode 100644 index 0000000..ad50261 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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); + diff --git a/src/prometheus_px4/src/modules/commander/esc_calibration.cpp b/src/prometheus_px4/src/modules/commander/esc_calibration.cpp new file mode 100644 index 0000000..d7b181e --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/esc_calibration.cpp @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * 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 + */ + +#include "esc_calibration.h" +#include "calibration_messages.h" +#include "calibration_routines.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +bool check_battery_disconnected(orb_advert_t *mavlink_log_pub) +{ + uORB::SubscriptionData 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 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; +} diff --git a/src/prometheus_px4/src/modules/commander/esc_calibration.h b/src/prometheus_px4/src/modules/commander/esc_calibration.h new file mode 100644 index 0000000..a304376 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/esc_calibration.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 + */ + +#ifndef ESC_CALIBRATION_H_ +#define ESC_CALIBRATION_H_ + +#include + +int do_esc_calibration(orb_advert_t *mavlink_log_pub); + +bool check_battery_disconnected(orb_advert_t *mavlink_log_pub); + +#endif diff --git a/src/prometheus_px4/src/modules/commander/factory_calibration_storage.cpp b/src/prometheus_px4/src/modules/commander/factory_calibration_storage.cpp new file mode 100644 index 0000000..075b9b8 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/factory_calibration_storage.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#include +#include + +#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"), ¶m); + _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; + } +} + diff --git a/src/prometheus_px4/src/modules/commander/factory_calibration_storage.h b/src/prometheus_px4/src/modules/commander/factory_calibration_storage.h new file mode 100644 index 0000000..bfb7596 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/factory_calibration_storage.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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}; +}; diff --git a/src/prometheus_px4/src/modules/commander/failure_detector/CMakeLists.txt b/src/prometheus_px4/src/modules/commander/failure_detector/CMakeLists.txt new file mode 100644 index 0000000..8c68bc6 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/failure_detector/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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 +) diff --git a/src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.cpp b/src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.cpp new file mode 100644 index 0000000..2799e2d --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * 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 +* +*/ + +#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; + } +} diff --git a/src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.hpp b/src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.hpp new file mode 100644 index 0000000..e158aff --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/failure_detector/FailureDetector.hpp @@ -0,0 +1,104 @@ + +/**************************************************************************** + * + * 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 +* +*/ + +#pragma once + +#include +#include +#include +#include + +// subscriptions +#include +#include +#include +#include +#include +#include +#include + +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) _param_fd_fail_p, + (ParamInt) _param_fd_fail_r, + (ParamFloat) _param_fd_fail_r_ttri, + (ParamFloat) _param_fd_fail_p_ttri, + (ParamBool) _param_fd_ext_ats_en, + (ParamInt) _param_fd_ext_ats_trig, + (ParamInt) _param_escs_en + ) +}; diff --git a/src/prometheus_px4/src/modules/commander/failure_detector/failure_detector_params.c b/src/prometheus_px4/src/modules/commander/failure_detector/failure_detector_params.c new file mode 100644 index 0000000..e841efc --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/failure_detector/failure_detector_params.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 + */ + +#include +#include + +/** + * 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); diff --git a/src/prometheus_px4/src/modules/commander/gyro_calibration.cpp b/src/prometheus_px4/src/modules/commander/gyro_calibration.cpp new file mode 100644 index 0000000..3187fea --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/gyro_calibration.cpp @@ -0,0 +1,298 @@ +/**************************************************************************** +* +* 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 +#include "factory_calibration_storage.h" +#include "gyro_calibration.h" +#include "calibration_messages.h" +#include "calibration_routines.h" +#include "commander_helper.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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 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 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; +} diff --git a/src/prometheus_px4/src/modules/commander/gyro_calibration.h b/src/prometheus_px4/src/modules/commander/gyro_calibration.h new file mode 100644 index 0000000..1abb51d --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/gyro_calibration.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 +#include + +int do_gyro_calibration(orb_advert_t *mavlink_log_pub); + +#endif /* GYRO_CALIBRATION_H_ */ diff --git a/src/prometheus_px4/src/modules/commander/level_calibration.cpp b/src/prometheus_px4/src/modules/commander/level_calibration.cpp new file mode 100644 index 0000000..9e4092c --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/level_calibration.cpp @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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; + } +} diff --git a/src/prometheus_px4/src/modules/commander/level_calibration.h b/src/prometheus_px4/src/modules/commander/level_calibration.h new file mode 100644 index 0000000..454a538 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/level_calibration.h @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 +#include + +int do_level_calibration(orb_advert_t *mavlink_log_pub); diff --git a/src/prometheus_px4/src/modules/commander/lm_fit.cpp b/src/prometheus_px4/src/modules/commander/lm_fit.cpp new file mode 100644 index 0000000..39eb571 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/lm_fit.cpp @@ -0,0 +1,356 @@ +/**************************************************************************** + * + * 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 ¶ms, 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 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 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 ¶ms, 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 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 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 ¶ms, + 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; +} + diff --git a/src/prometheus_px4/src/modules/commander/lm_fit.hpp b/src/prometheus_px4/src/modules/commander/lm_fit.hpp new file mode 100644 index 0000000..52fa674 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/lm_fit.hpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * 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 +#include + +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 ¶ms, + bool full_ellipsoid); diff --git a/src/prometheus_px4/src/modules/commander/mag_calibration.cpp b/src/prometheus_px4/src/modules/commander/mag_calibration.cpp new file mode 100644 index 0000000..b047a96 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/mag_calibration.cpp @@ -0,0 +1,1043 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 mag_calibration.cpp + * + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" +#include "lm_fit.hpp" +#include "calibration_messages.h" +#include "factory_calibration_storage.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; + +static constexpr char sensor_name[] {"mag"}; +static constexpr int MAX_MAGS = 4; +static constexpr float MAG_SPHERE_RADIUS_DEFAULT = 0.2f; +static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer +static constexpr unsigned int calibraton_duration_s = 42; ///< The total duration the routine is allowed to take + +calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask); + +/// Data passed to calibration worker routine +struct mag_worker_data_t { + orb_advert_t *mavlink_log_pub; + orb_advert_t mag_worker_data_pub; + bool append_to_existing_calibration; + unsigned last_mag_progress; + unsigned done_count; + unsigned calibration_sides; ///< The total number of sides + bool side_data_collected[detect_orientation_side_count]; + unsigned int calibration_points_perside; + uint64_t calibration_interval_perside_us; + unsigned int calibration_counter_total[MAX_MAGS]; + + float *x[MAX_MAGS]; + float *y[MAX_MAGS]; + float *z[MAX_MAGS]; + + calibration::Magnetometer calibration[MAX_MAGS] {}; +}; + +int do_mag_calibration(orb_advert_t *mavlink_log_pub) +{ + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); + + int result = PX4_OK; + + // Collect: As defined by configuration + // start with a full mask, all six bits set + int32_t cal_mask = (1 << 6) - 1; + param_get(param_find("CAL_MAG_SIDES"), &cal_mask); + + // Calibrate all mags at the same time + if (result == PX4_OK) { + switch (mag_calibrate_all(mavlink_log_pub, cal_mask)) { + case calibrate_return_cancelled: + // Cancel message already displayed, we're done here + result = PX4_ERROR; + break; + + case calibrate_return_ok: + /* if there is a any preflight-check system response, let the barrage of messages through */ + px4_usleep(200000); + + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); + px4_usleep(20000); + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); + px4_usleep(20000); + break; + + default: + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); + px4_usleep(20000); + break; + } + } + + /* give this message enough time to propagate */ + px4_usleep(600000); + + return result; +} + +static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, + unsigned max_count, float mag_sphere_radius) +{ + float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; + + for (size_t i = 0; i < count; i++) { + float dx = sx - x[i]; + float dy = sy - y[i]; + float dz = sz - z[i]; + float dist = sqrtf(dx * dx + dy * dy + dz * dz); + + if (dist < min_sample_dist) { + PX4_DEBUG("rejected X: %.3f Y: %.3f Z: %.3f (%.3f < %.3f) (%u/%u) ", (double)sx, (double)sy, (double)sz, (double)dist, + (double)min_sample_dist, count, max_count); + + return true; + } + } + + return false; +} + +static unsigned progress_percentage(mag_worker_data_t *worker_data) +{ + return 100 * ((float)worker_data->done_count) / worker_data->calibration_sides; +} + +// Returns calibrate_return_error if any parameter is not finite +// Logs if parameters are out of range +static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z, + float sphere_radius, + float diag_x, float diag_y, float diag_z, + float offdiag_x, float offdiag_y, float offdiag_z, + orb_advert_t *mavlink_log_pub, uint8_t cur_mag) +{ + float must_be_finite[] = {offset_x, offset_y, offset_z, + sphere_radius, + diag_x, diag_y, diag_z, + offdiag_x, offdiag_y, offdiag_z + }; + + float should_be_not_huge[] = {offset_x, offset_y, offset_z}; + float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z}; + + // Make sure every parameter is finite + const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite); + + for (unsigned i = 0; i < num_finite; ++i) { + if (!PX4_ISFINITE(must_be_finite[i])) { + calibration_log_emergency(mavlink_log_pub, "Retry calibration (sphere NaN, %" PRIu8 ")", cur_mag); + return calibrate_return_error; + } + } + + // earth field between 0.25 and 0.65 Gauss + if (sphere_radius < 0.2f || sphere_radius >= 0.7f) { + calibration_log_emergency(mavlink_log_pub, "Retry calibration (mag %" PRIu8 " sphere radius invalid %.3f)", cur_mag, + (double)sphere_radius); + return calibrate_return_error; + } + + // Notify if a parameter which should be positive is non-positive + const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive); + + for (unsigned i = 0; i < num_positive; ++i) { + if (should_be_positive[i] <= 0.0f) { + calibration_log_emergency(mavlink_log_pub, "Retry calibration (mag %" PRIu8 " with non-positive scale)", cur_mag); + return calibrate_return_error; + } + } + + // Notify if offsets are too large + const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge); + + for (unsigned i = 0; i < num_not_huge; ++i) { + // maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, + // so an offset larger than ~1.3 Ga means the mag will saturate in some directions. + static constexpr float MAG_MAX_OFFSET_LEN = 1.3f; + + if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) { + calibration_log_critical(mavlink_log_pub, "Warning: mag %" PRIu8 " with large offsets", cur_mag); + break; + } + } + + return calibrate_return_ok; +} + +static float get_sphere_radius() +{ + // if GPS is available use real field intensity from world magnetic model + uORB::SubscriptionMultiArray gps_subs{ORB_ID::vehicle_gps_position}; + + for (auto &gps_sub : gps_subs) { + vehicle_gps_position_s gps; + + if (gps_sub.copy(&gps)) { + if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) { + const double lat = gps.lat / 1.e7; + const double lon = gps.lon / 1.e7; + + // magnetic field data returned by the geo library using the current GPS position + return get_mag_strength_gauss(lat, lon); + } + } + } + + return MAG_SPHERE_RADIUS_DEFAULT; +} + +static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data) +{ + const hrt_abstime calibration_started = hrt_absolute_time(); + calibrate_return result = calibrate_return_ok; + + mag_worker_data_t *worker_data = (mag_worker_data_t *)(data); + + float mag_sphere_radius = get_sphere_radius(); + + // notify user to start rotating + set_tune(tune_control_s::TUNE_ID_SINGLE_BEEP); + + calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle"); + + /* + * Detect if the system is rotating. + * + * We're detecting this as a general rotation on any axis, not necessary on the one we + * asked the user for. This is because we really just need two roughly orthogonal axes + * for a good result, so we're not constraining the user more than we have to. + */ + + const hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_us * 5; + hrt_abstime last_gyro = 0; + float gyro_x_integral = 0.0f; + float gyro_y_integral = 0.0f; + float gyro_z_integral = 0.0f; + + static constexpr float gyro_int_thresh_rad = 0.5f; + + uORB::SubscriptionBlocking gyro_sub{ORB_ID(sensor_gyro)}; + + while (fabsf(gyro_x_integral) < gyro_int_thresh_rad && + fabsf(gyro_y_integral) < gyro_int_thresh_rad && + fabsf(gyro_z_integral) < gyro_int_thresh_rad) { + + /* abort on request */ + if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { + result = calibrate_return_cancelled; + return result; + } + + /* abort with timeout */ + if (hrt_absolute_time() > detection_deadline) { + result = calibrate_return_error; + PX4_ERR("gyro int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral); + calibration_log_critical(worker_data->mavlink_log_pub, "Failed: This calibration requires rotation."); + break; + } + + /* Wait clocking for new data on all gyro */ + sensor_gyro_s gyro; + + if (gyro_sub.updateBlocking(gyro, 1000_ms)) { + + /* ensure we have a valid first timestamp */ + if (last_gyro > 0) { + + /* integrate */ + float delta_t = (gyro.timestamp - last_gyro) / 1e6f; + gyro_x_integral += gyro.x * delta_t; + gyro_y_integral += gyro.y * delta_t; + gyro_z_integral += gyro.z * delta_t; + } + + last_gyro = gyro.timestamp; + } + } + + uORB::SubscriptionBlocking mag_sub[MAX_MAGS] { + {ORB_ID(sensor_mag), 0, 0}, + {ORB_ID(sensor_mag), 0, 1}, + {ORB_ID(sensor_mag), 0, 2}, + {ORB_ID(sensor_mag), 0, 3}, + }; + + uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_us; + unsigned poll_errcount = 0; + unsigned calibration_counter_side = 0; + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter_side < worker_data->calibration_points_perside) { + + if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { + result = calibrate_return_cancelled; + break; + } + + if (mag_sub[0].updatedBlocking(1000_ms)) { + bool rejected = false; + Vector3f new_samples[MAX_MAGS] {}; + + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data->calibration[cur_mag].device_id() != 0) { + bool updated = false; + sensor_mag_s mag; + + while (mag_sub[cur_mag].update(&mag)) { + if (worker_data->append_to_existing_calibration) { + // keep and update the existing calibration when we are not doing a full 6-axis calibration + const Matrix3f &scale = worker_data->calibration[cur_mag].scale(); + const Vector3f &offset = worker_data->calibration[cur_mag].offset(); + + const Vector3f m{scale *(Vector3f{mag.x, mag.y, mag.z} - offset)}; + + mag.x = m(0); + mag.y = m(1); + mag.z = m(2); + } + + // Check if this measurement is good to go in + bool reject = reject_sample(mag.x, mag.y, mag.z, + worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], + worker_data->calibration_counter_total[cur_mag], + worker_data->calibration_sides * worker_data->calibration_points_perside, + mag_sphere_radius); + + if (!reject) { + new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z}; + updated = true; + break; + } + } + + // require an update for all valid mags + if (!updated) { + rejected = true; + } + } + } + + // Keep calibration of all mags in lockstep + if (!rejected) { + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data->calibration[cur_mag].device_id() != 0) { + worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](0); + worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1); + worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](2); + worker_data->calibration_counter_total[cur_mag]++; + } + } + + hrt_abstime now = hrt_absolute_time(); + mag_worker_data_s status; + status.timestamp = now; + status.timestamp_sample = now; + status.done_count = worker_data->done_count; + status.calibration_points_perside = worker_data->calibration_points_perside; + status.calibration_interval_perside_us = worker_data->calibration_interval_perside_us; + + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + status.calibration_counter_total[cur_mag] = worker_data->calibration_counter_total[cur_mag]; + status.side_data_collected[cur_mag] = worker_data->side_data_collected[cur_mag]; + + if (worker_data->calibration[cur_mag].device_id() != 0) { + const unsigned int sample = worker_data->calibration_counter_total[cur_mag] - 1; + status.x[cur_mag] = worker_data->x[cur_mag][sample]; + status.y[cur_mag] = worker_data->y[cur_mag][sample]; + status.z[cur_mag] = worker_data->z[cur_mag][sample]; + + } else { + status.x[cur_mag] = 0.f; + status.y[cur_mag] = 0.f; + status.z[cur_mag] = 0.f; + } + } + + if (worker_data->mag_worker_data_pub == nullptr) { + worker_data->mag_worker_data_pub = orb_advertise(ORB_ID(mag_worker_data), &status); + + } else { + orb_publish(ORB_ID(mag_worker_data), worker_data->mag_worker_data_pub, &status); + } + + calibration_counter_side++; + + unsigned new_progress = progress_percentage(worker_data) + + (unsigned)((100 / worker_data->calibration_sides) * ((float)calibration_counter_side / (float) + worker_data->calibration_points_perside)); + + if (new_progress - worker_data->last_mag_progress > 0) { + // Progress indicator for side + calibration_log_info(worker_data->mavlink_log_pub, + "[cal] %s side calibration: progress <%u>", + detect_orientation_str(orientation), new_progress); + px4_usleep(10000); + + worker_data->last_mag_progress = new_progress; + } + } + + PX4_DEBUG("side counter %u / %u", calibration_counter_side, worker_data->calibration_points_perside); + + } else { + poll_errcount++; + } + + if (poll_errcount > worker_data->calibration_points_perside * 3) { + result = calibrate_return_error; + calibration_log_info(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG); + break; + } + } + + if (result == calibrate_return_ok) { + calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side done, rotate to a different side", + detect_orientation_str(orientation)); + + worker_data->done_count++; + px4_usleep(20000); + calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data)); + } + + return result; +} + +calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask) +{ + // We should not try to subscribe if the topic doesn't actually exist and can be counted. + const unsigned orb_mag_count = orb_group_count(ORB_ID(sensor_mag)); + + // Warn that we will not calibrate more than MAX_GYROS gyroscopes + if (orb_mag_count > MAX_MAGS) { + calibration_log_critical(mavlink_log_pub, "Detected %u mags, but will calibrate only %u", orb_mag_count, MAX_MAGS); + + } else if (orb_mag_count < 1) { + calibration_log_critical(mavlink_log_pub, "No mags found"); + return calibrate_return_error; + } + + calibrate_return result = calibrate_return_ok; + + mag_worker_data_t worker_data{}; + + worker_data.mag_worker_data_pub = nullptr; + + // keep and update the existing calibration when we are not doing a full 6-axis calibration + worker_data.append_to_existing_calibration = cal_mask < ((1 << 6) - 1); + worker_data.mavlink_log_pub = mavlink_log_pub; + worker_data.last_mag_progress = 0; + worker_data.calibration_sides = 0; + worker_data.done_count = 0; + worker_data.calibration_points_perside = calibration_total_points / detect_orientation_side_count; + worker_data.calibration_interval_perside_us = (calibraton_duration_s / detect_orientation_side_count) * 1000 * 1000; + + for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) / sizeof(worker_data.side_data_collected[0])); i++) { + + if ((cal_mask & (1 << i)) > 0) { + // mark as missing + worker_data.side_data_collected[i] = false; + worker_data.calibration_sides++; + + } else { + // mark as completed from the beginning + worker_data.side_data_collected[i] = true; + + calibration_log_info(mavlink_log_pub, + "[cal] %s side done, rotate to a different side", + detect_orientation_str(static_cast(i))); + px4_usleep(100000); + } + } + + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + // Initialize to no memory allocated + worker_data.x[cur_mag] = nullptr; + worker_data.y[cur_mag] = nullptr; + worker_data.z[cur_mag] = nullptr; + worker_data.calibration_counter_total[cur_mag] = 0; + } + + const unsigned int calibration_points_maxcount = worker_data.calibration_sides * worker_data.calibration_points_perside; + + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + + uORB::SubscriptionData mag_sub{ORB_ID(sensor_mag), cur_mag}; + + if (mag_sub.advertised() && (mag_sub.get().device_id != 0) && (mag_sub.get().timestamp > 0)) { + worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id, mag_sub.get().is_external); + } + + // reset calibration index to match uORB numbering + worker_data.calibration[cur_mag].set_calibration_index(cur_mag); + + if (worker_data.calibration[cur_mag].device_id() != 0) { + worker_data.x[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.y[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.z[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); + + if (worker_data.x[cur_mag] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) { + calibration_log_critical(mavlink_log_pub, "ERROR: out of memory"); + result = calibrate_return_error; + break; + } + + } else { + break; + } + } + + if (result == calibrate_return_ok) { + result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output + worker_data.side_data_collected, // Sides to calibrate + mag_calibration_worker, // Calibration worker + &worker_data, // Opaque data for calibration worked + true); // true: lenient still detection + } + + // calibration values for each mag + Vector3f sphere[MAX_MAGS]; + Vector3f diag[MAX_MAGS]; + Vector3f offdiag[MAX_MAGS]; + float sphere_radius[MAX_MAGS]; + + const float mag_sphere_radius = get_sphere_radius(); + + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + sphere_radius[cur_mag] = mag_sphere_radius; + sphere[cur_mag].zero(); + diag[cur_mag] = Vector3f{1.f, 1.f, 1.f}; + offdiag[cur_mag].zero(); + } + + if (result == calibrate_return_ok) { + // Sphere fit the data to get calibration values + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data.calibration[cur_mag].device_id() != 0) { + // Mag in this slot is available and we should have values for it to calibrate + + // Estimate only the offsets if two-sided calibration is selected, as the problem is not constrained + // enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration + // is already close) + bool sphere_fit_only = worker_data.calibration_sides <= 2; + + sphere_params sphere_data; + sphere_data.radius = sphere_radius[cur_mag]; + sphere_data.offset = matrix::Vector3f(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2)); + sphere_data.diag = matrix::Vector3f(diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2)); + sphere_data.offdiag = matrix::Vector3f(offdiag[cur_mag](0), offdiag[cur_mag](1), offdiag[cur_mag](2)); + + bool sphere_fit_success = false; + bool ellipsoid_fit_success = false; + int ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], + worker_data.calibration_counter_total[cur_mag], sphere_data, false); + + if (ret == PX4_OK) { + sphere_fit_success = true; + PX4_INFO("Mag: %" PRIu8 " sphere radius: %.4f", cur_mag, (double)sphere_data.radius); + + if (!sphere_fit_only) { + int ellipsoid_ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], + worker_data.calibration_counter_total[cur_mag], sphere_data, true); + + if (ellipsoid_ret == PX4_OK) { + ellipsoid_fit_success = true; + } + } + } + + sphere_radius[cur_mag] = sphere_data.radius; + + for (int i = 0; i < 3; i++) { + sphere[cur_mag](i) = sphere_data.offset(i); + diag[cur_mag](i) = sphere_data.diag(i); + offdiag[cur_mag](i) = sphere_data.offdiag(i); + } + + if (!sphere_fit_success && !ellipsoid_fit_success) { + calibration_log_emergency(mavlink_log_pub, "Retry calibration (unable to fit mag %" PRIu8 ")", cur_mag); + result = calibrate_return_error; + break; + } + + result = check_calibration_result(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2), + sphere_radius[cur_mag], + diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2), + offdiag[cur_mag](0), offdiag[cur_mag](1), offdiag[cur_mag](2), + mavlink_log_pub, cur_mag); + + if (result == calibrate_return_error) { + break; + } + } + } + } + + +#if 0 + + // DO NOT REMOVE! Critical validation data! + if (result == calibrate_return_ok) { + // Print uncalibrated data points + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data.calibration_counter_total[cur_mag] == 0) { + continue; + } + + printf("MAG %" PRIu8 " with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); + printf("RAW -> CALIBRATED\n"); + + float scale_data[9] { + diag[cur_mag](0), offdiag[cur_mag](0), offdiag[cur_mag](1), + offdiag[cur_mag](0), diag[cur_mag](1), offdiag[cur_mag](2), + offdiag[cur_mag](1), offdiag[cur_mag](2), diag[cur_mag](2) + }; + + const Matrix3f scale{scale_data}; + const Vector3f &offset = sphere[cur_mag]; + + for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { + float x = worker_data.x[cur_mag][i]; + float y = worker_data.y[cur_mag][i]; + float z = worker_data.z[cur_mag][i]; + + // apply calibration + const Vector3f cal{scale *(Vector3f{x, y, z} - offset)}; + + printf("[%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]\n", + (double)x, (double)y, (double)z, + (double)cal(0), (double)cal(1), (double)cal(2)); + } + + printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]); + } + } + +#endif // DO NOT REMOVE! Critical validation data! + + + // Attempt to automatically determine external mag rotations + if (result == calibrate_return_ok) { + int32_t param_cal_mag_rot_auto = 0; + param_get(param_find("CAL_MAG_ROT_AUTO"), ¶m_cal_mag_rot_auto); + + if ((worker_data.calibration_sides >= 3) && (param_cal_mag_rot_auto == 1)) { + + // find first internal mag to use as reference + int internal_index = -1; + + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (!worker_data.calibration[cur_mag].external() && (worker_data.calibration[cur_mag].device_id() != 0)) { + internal_index = cur_mag; + break; + } + } + + // only proceed if there's a valid internal + if (internal_index >= 0) { + + const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); + + // apply new calibrations to all raw sensor data before comparison + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data.calibration[cur_mag].device_id() != 0) { + + float scale_data[9] { + diag[cur_mag](0), offdiag[cur_mag](0), offdiag[cur_mag](1), + offdiag[cur_mag](0), diag[cur_mag](1), offdiag[cur_mag](2), + offdiag[cur_mag](1), offdiag[cur_mag](2), diag[cur_mag](2) + }; + const Matrix3f scale{scale_data}; + + const Vector3f &offset{sphere[cur_mag]}; + + for (unsigned i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { + + const Vector3f sample{worker_data.x[cur_mag][i], worker_data.y[cur_mag][i], worker_data.z[cur_mag][i]}; + + // apply calibration + Vector3f m{scale *(sample - offset)}; + + if (!worker_data.calibration[cur_mag].external()) { + // rotate internal mag data to board + m = board_rotation * m; + } + + // store back in worker_data + worker_data.x[cur_mag][i] = m(0); + worker_data.y[cur_mag][i] = m(1); + worker_data.z[cur_mag][i] = m(2); + } + } + } + + // external mags try all rotations and compute mean square error (MSE) compared with first internal mag + for (int cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if ((worker_data.calibration[cur_mag].device_id() != 0) && (cur_mag != internal_index)) { + + const int last_sample_index = math::min(worker_data.calibration_counter_total[internal_index], + worker_data.calibration_counter_total[cur_mag]); + + float MSE[ROTATION_MAX] {}; // mean square error for each rotation + + float min_mse = FLT_MAX; + Rotation best_rotation = ROTATION_NONE; + + for (int r = ROTATION_NONE; r < ROTATION_MAX; r++) { + + switch (r) { + case ROTATION_ROLL_90_PITCH_68_YAW_293: // skip + + // FALLTHROUGH + case ROTATION_PITCH_180_YAW_90: // skip 26, same as 14 ROTATION_ROLL_180_YAW_270 + + // FALLTHROUGH + case ROTATION_PITCH_180_YAW_270: // skip 27, same as 10 ROTATION_ROLL_180_YAW_90 + MSE[r] = FLT_MAX; + break; + + default: + float diff_sum = 0.f; + + for (int i = 0; i < last_sample_index; i++) { + float x = worker_data.x[cur_mag][i]; + float y = worker_data.y[cur_mag][i]; + float z = worker_data.z[cur_mag][i]; + rotate_3f((enum Rotation)r, x, y, z); + + Vector3f diff = Vector3f{x, y, z} - Vector3f{worker_data.x[internal_index][i], worker_data.y[internal_index][i], worker_data.z[internal_index][i]}; + + diff_sum += diff.norm_squared(); + } + + // compute mean squared error + MSE[r] = diff_sum / last_sample_index; + + if (MSE[r] < min_mse) { + min_mse = MSE[r]; + best_rotation = (Rotation)r; + } + + break; + } + } + + + // Check that the best rotation is at least twice as good as the next best + bool smallest_check_passed = true; + + for (int r = ROTATION_NONE; r < ROTATION_MAX; r++) { + if (r != best_rotation) { + if (MSE[r] < (min_mse * 2.f)) { + smallest_check_passed = false; + } + } + } + + + // Check that the average error across all samples (relative to internal mag) is less than the minimum earth field (~0.25 Gauss) + const float mag_error_gs = sqrt(min_mse / last_sample_index); + bool total_error_check_passed = (mag_error_gs < 0.25f); + +#if defined(DEBUG_BUILD) + bool print_all_mse = true; +#else + bool print_all_mse = false; +#endif // DEBUG_BUILD + + if (worker_data.calibration[cur_mag].external()) { + + switch (worker_data.calibration[cur_mag].rotation_enum()) { + case ROTATION_ROLL_90_PITCH_68_YAW_293: + PX4_INFO("[cal] External Mag: %d (%" PRIu32 "), keeping manually configured rotation %" PRIu8, cur_mag, + worker_data.calibration[cur_mag].device_id(), worker_data.calibration[cur_mag].rotation_enum()); + continue; + + default: + break; + } + + if (smallest_check_passed && total_error_check_passed) { + if (best_rotation != worker_data.calibration[cur_mag].rotation_enum()) { + calibration_log_info(mavlink_log_pub, "[cal] External Mag: %d (%" PRIu32 ")), determined rotation: %" PRIu8, cur_mag, + worker_data.calibration[cur_mag].device_id(), best_rotation); + + worker_data.calibration[cur_mag].set_rotation(best_rotation); + + } else { + PX4_INFO("[cal] External Mag: %d (%" PRIu32 ")), no rotation change: %d", cur_mag, + worker_data.calibration[cur_mag].device_id(), best_rotation); + } + + } else { + PX4_ERR("External Mag: %d (%" PRIu32 ")), determining rotation failed", cur_mag, + worker_data.calibration[cur_mag].device_id()); + print_all_mse = true; + } + + } else { + // non-primary internal mags, warn if there seems to be a rotation relative to the first primary (internal_index) + if (best_rotation != ROTATION_NONE) { + calibration_log_critical(mavlink_log_pub, + "[cal] Internal Mag: %d (%" PRIu32 ") rotation %d relative to primary %d (%" PRIu32 ")", + cur_mag, worker_data.calibration[cur_mag].device_id(), best_rotation, + internal_index, worker_data.calibration[internal_index].device_id()); + + print_all_mse = true; + } + } + + if (print_all_mse) { + for (int r = ROTATION_NONE; r < ROTATION_MAX; r++) { + PX4_ERR("%s Mag: %d (%" PRIu32 "), rotation: %d, MSE: %.3f", + worker_data.calibration[cur_mag].external() ? "External" : "Internal", + cur_mag, worker_data.calibration[cur_mag].device_id(), r, (double)MSE[r]); + } + } + } + } + } + } + } + + + // Data points are no longer needed + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + free(worker_data.x[cur_mag]); + free(worker_data.y[cur_mag]); + free(worker_data.z[cur_mag]); + } + + FactoryCalibrationStorage factory_storage; + + if (result == calibrate_return_ok && factory_storage.open() != PX4_OK) { + calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage"); + result = calibrate_return_error; + } + + if (result == calibrate_return_ok) { + bool param_save = false; + bool failed = true; + + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + + auto ¤t_cal = worker_data.calibration[cur_mag]; + + if (current_cal.device_id() != 0) { + if (worker_data.append_to_existing_calibration) { + // Update calibration + // The formula for applying the calibration is: + // mag_value = (mag_readout - (offset_existing + offset_new/scale_existing)) * scale_existing + Vector3f offset = current_cal.offset() + sphere[cur_mag].edivide(current_cal.scale().diag()); + current_cal.set_offset(offset); + + } else { + current_cal.set_offset(sphere[cur_mag]); + current_cal.set_scale(diag[cur_mag]); + current_cal.set_offdiagonal(offdiag[cur_mag]); + } + + current_cal.set_calibration_index(cur_mag); + + current_cal.PrintStatus(); + + if (current_cal.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) { + result = calibrate_return_error; + } + } + + return result; +} + +int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians, float latitude, float longitude) +{ + // magnetometer quick calibration + // if GPS available use world magnetic model to zero mag offsets + bool mag_earth_available = false; + + if (PX4_ISFINITE(latitude) && PX4_ISFINITE(longitude)) { + mag_earth_available = true; + + } else { + uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + vehicle_gps_position_s gps; + + if (vehicle_gps_position_sub.copy(&gps)) { + if ((gps.timestamp != 0) && (gps.eph < 1000)) { + latitude = gps.lat / 1.e7f; + longitude = gps.lon / 1.e7f; + mag_earth_available = true; + } + } + } + + if (!mag_earth_available) { + calibration_log_critical(mavlink_log_pub, "GPS required for mag quick cal"); + return PX4_ERROR; + + } else { + + // magnetic field data returned by the geo library using the current GPS position + const float mag_declination_gps = get_mag_declination_radians(latitude, longitude); + const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude); + const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude); + + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, + 0, 0); + + uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + vehicle_attitude_s attitude{}; + vehicle_attitude_sub.copy(&attitude); + + if (hrt_elapsed_time(&attitude.timestamp) > 1_s) { + calibration_log_critical(mavlink_log_pub, "attitude required for mag quick cal"); + 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; + } + + calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees", + (double)math::radians(heading_radians)); + + matrix::Eulerf euler{matrix::Quatf{attitude.q}}; + euler(2) = heading_radians; + + const Vector3f expected_field = Dcmf(euler).transpose() * mag_earth_pred; + + bool param_save = false; + bool failed = true; + + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + uORB::Subscription mag_sub{ORB_ID(sensor_mag), cur_mag}; + sensor_mag_s mag{}; + mag_sub.copy(&mag); + + if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) { + + calibration::Magnetometer cal{mag.device_id, mag.is_external}; + + // force calibration index to uORB index + cal.set_calibration_index(cur_mag); + + // use any existing scale and store the offset to the expected earth field + const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field); + cal.set_offset(offset); + + // save new calibration + if (cal.ParametersSave()) { + cal.PrintStatus(); + param_save = true; + failed = false; + + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } + } + } + + if (param_save) { + param_notify_changes(); + } + + if (!failed && factory_storage.store() != PX4_OK) { + failed = true; + } + + if (!failed) { + calibration_log_info(mavlink_log_pub, "Mag quick calibration finished"); + return PX4_OK; + } + } + + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); + return PX4_ERROR; +} diff --git a/src/prometheus_px4/src/modules/commander/mag_calibration.h b/src/prometheus_px4/src/modules/commander/mag_calibration.h new file mode 100644 index 0000000..95ae3ec --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/mag_calibration.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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 +#include +#include + +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_ */ diff --git a/src/prometheus_px4/src/modules/commander/mag_calibration_test.cpp b/src/prometheus_px4/src/modules/commander/mag_calibration_test.cpp new file mode 100644 index 0000000..3343a28 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/mag_calibration_test.cpp @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * 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 + */ + +#include +#include +#include + +#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(M_PI_F / d); + const float d_theta = M_PI_F / static_cast(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(m_theta); + const int m_phi = static_cast(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(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); +} diff --git a/src/prometheus_px4/src/modules/commander/mag_calibration_test_data.h b/src/prometheus_px4/src/modules/commander/mag_calibration_test_data.h new file mode 100644 index 0000000..e66925d --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/mag_calibration_test_data.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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 data for the Magnetometer calibration unit tests + */ + +static constexpr float mag_data1_x[231] = {-0.4820142686367035f, -0.49314647912979126f, -0.48948222398757935f, -0.4931904077529907f, -0.47832345962524414f, -0.4709412157535553f, -0.4264504015445709f, -0.3745213747024536f, -0.3448501527309418f, -0.3077530264854431f, -0.27809011936187744f, -0.22989600896835327f, -0.20765912532806396f, -0.15944115817546844f, -0.12978695333003998f, -0.08528247475624084f, -0.01854165457189083f, 0.011124793440103531f, 0.037078678607940674f, 0.05191200226545334f, 0.05932694301009178f, 0.06674399971961975f, 0.059328000992536545f, 0.0556190088391304f, 0.04820830374956131f, 0.02595600113272667f, -0.022249983623623848f, -0.04079090803861618f, -0.059328000992536545f, -0.09640628844499588f, -0.1260720044374466f, -0.17798399925231934f, -0.2113559991121292f, -0.25585201382637024f, -0.2780950367450714f, -0.32633310556411743f, -0.37086614966392517f, -0.31892216205596924f, -0.27442631125450134f, -0.21510623395442963f, -0.14833854138851166f, -0.1260945051908493f, -0.07417456805706024f, -0.051917560398578644f, 0.003708463627845049f, 0.05563092231750488f, 0.07788468152284622f, 0.09271159023046494f, 0.10383698344230652f, 0.10012851655483246f, 0.09271159023046494f, 0.05563092231750488f, 0.007417456246912479f, -0.07046583294868469f, -0.15204700827598572f, -0.17802532017230988f, -0.22992472350597382f, -0.25961098074913025f, -0.28554657101631165f, -0.33008861541748047f, -0.35229772329330444f, -0.3893955945968628f, -0.4339211881160736f, -0.43760645389556885f, -0.4524325430393219f, -0.4636075794696808f, -0.4450235366821289f, -0.4339057207107544f, -0.4116835296154022f, -0.29668235778808594f, -0.2595970332622528f, -0.22251969575881958f, -0.1965731382369995f, -0.18172118067741394f, -0.3931955397129059f, -0.4228706657886505f, -0.4562224745750427f, -0.4710506200790405f, -0.48222094774246216f, -0.48221227526664734f, -0.47846877574920654f, -0.47476813197135925f, -0.4599316418170929f, -0.44880422949790955f, -0.43399885296821594f, -0.4080331027507782f, -0.3931955397129059f, -0.35606977343559265f, -0.3338213264942169f, -0.29673540592193604f, -0.2781844437122345f, -0.2522251009941101f, -0.20771105587482452f, -0.14836770296096802f, -0.12983104586601257f, -0.0964355617761612f, -0.07047339528799057f, -0.04080038517713547f, 0.01854562945663929f, 0.05564187467098236f, 0.07418385148048401f, 0.08531142771244049f, 0.11128374934196472f, 0.10014640539884567f, 0.10385739058256149f, 0.10386482626199722f, 0.10386482626199722f, 0.08902062475681305f, 0.07789862155914307f, 0.07047970592975616f, 0.0482238233089447f, 0.029673539102077484f, 0.0f, 0.029676198959350586f, 0.014838632196187973f, 0.014838099479675293f, 0.0f, 0.0074191829189658165f, -0.0037095914594829082f, -0.011129574850201607f, -0.0037099244073033333f, -0.0074191829189658165f, -0.014839697629213333f, -0.007419581990689039f, -0.003709858050569892f, 0.011128774844110012f, 0.01854928955435753f, 0.025967607274651527f, 0.025967607274651527f, 0.03338632360100746f, 0.04451829940080643f, 0.04822901636362076f, 0.04451509937644005f, 0.05193427950143814f, 0.05935346335172653f, 0.055644869804382324f, 0.05935879051685333f, 0.06677983701229095f, 0.06306418776512146f, 0.07048476487398148f, 0.07420115172863007f, 0.07048856467008591f, 0.07048982381820679f, 0.05935879051685333f, 0.04451989009976387f, 0.037099242210388184f, 0.04080624133348465f, 0.03338751941919327f, 0.03338751941919327f, 0.022259945049881935f, 0.0f, -0.3969832956790924f, -0.4155339002609253f, -0.44155243039131165f, -0.45636165142059326f, -0.46747565269470215f, -0.4712366461753845f, -0.4675261080265045f, -0.4489411413669586f, -0.44893312454223633f, -0.4304364025592804f, -0.4192892611026764f, -0.3932802379131317f, -0.34507253766059875f, -0.3228156566619873f, -0.26716259121894836f, -0.2077707052230835f, -0.18179936707019806f, -0.14469745755195618f, -0.11502417922019958f, -0.08162566274404526f, -0.029683660715818405f, -0.014840764924883842f, 0.011130573228001595f, 0.03339231759309769f, 0.1075974628329277f, 0.11873892694711685f, 0.12986135482788086f, 0.11501797288656235f, 0.09275642782449722f, 0.048236820846796036f, -0.018553292378783226f, -0.07049616426229477f, -0.1410050243139267f, -0.20406784117221832f, -0.2263297736644745f, -0.2708536982536316f, -0.2931472957134247f, -0.3710324466228485f, -0.3970118761062622f, -0.4081430435180664f, -0.4601631164550781f, -0.46012163162231445f, -0.45270034670829773f, -0.44527098536491394f, -0.43414703011512756f, -0.41930443048477173f, -0.40077269077301025f, -0.378493994474411f, -0.37107253074645996f, -0.3562360405921936f, -0.3562617301940918f, -0.3376820683479309f, -0.3339712917804718f, -0.3191280961036682f, -0.31170654296875f, -0.31170654296875f, -0.31916263699531555f, -0.31170654296875f, -0.3079957365989685f, -0.3154173195362091f, -0.3079957365989685f, -0.31170654296875f, -0.3154173195362091f, -0.3265555500984192f, -0.3785075545310974f, -0.3970547318458557f, -0.4007583260536194f, -0.42674872279167175f, -0.4379129409790039f, -0.4415842294692993f, -0.44533517956733704f, -0.46015480160713196f, -0.46389082074165344f, -0.46015480160713196f, -0.4601631164550781f, -0.4601631164550781f, -0.460146427154541f, -0.45272472500801086f, -0.4416400194168091f, -0.4304751753807068f}; + +static constexpr float mag_data1_y[231] = {0.15201987326145172f, 0.10752817988395691f, 0.08528856933116913f, 0.04820658266544342f, 0.0037079339381307364f, -0.025957390666007996f, -0.08158181607723236f, -0.12978464365005493f, -0.13349038362503052f, -0.152022585272789f, -0.1557304561138153f, -0.16685999929904938f, -0.15574434399604797f, -0.15202529728412628f, -0.13349515199661255f, -0.11865388602018356f, -0.07787495106458664f, -0.05191570147871971f, -0.0037078680470585823f, 0.014832000248134136f, 0.06303487718105316f, 0.11865600198507309f, 0.14461199939250946f, 0.18168875575065613f, 0.19654153287410736f, 0.23360399901866913f, 0.28924980759620667f, 0.304077684879303f, 0.32630401849746704f, 0.3411298990249634f, 0.3596760034561157f, 0.3596760034561157f, 0.3670920133590698f, 0.3670920133590698f, 0.3559616804122925f, 0.344874769449234f, 0.2558976411819458f, 0.25587940216064453f, 0.24475860595703125f, 0.23735859990119934f, 0.23734167218208313f, 0.22622837126255035f, 0.23364987969398499f, 0.22992061078548431f, 0.22250781953334808f, 0.22252368927001953f, 0.22994525730609894f, 0.22621627151966095f, 0.22992472350597382f, 0.21879935264587402f, 0.22621627151966095f, 0.23735859990119934f, 0.2484847903251648f, 0.2633197009563446f, 0.2707178294658661f, 0.2744556963443756f, 0.2781347632408142f, 0.2855720520019531f, 0.28925496339797974f, 0.2818734347820282f, 0.28183817863464355f, 0.27813971042633057f, 0.2781546115875244f, 0.2818482220172882f, 0.2707178294658661f, 0.2670379877090454f, 0.2707226574420929f, 0.2596017122268677f, 0.2707468271255493f, 0.26330557465553284f, 0.2595970332622528f, 0.2707323133945465f, 0.25220704078674316f, 0.25218451023101807f, -0.09644418209791183f, -0.10386297106742859f, -0.1224011555314064f, -0.11868992447853088f, -0.1372475028038025f, -0.14837302267551422f, -0.14094428718090057f, -0.14836503565311432f, -0.16320154070854187f, -0.17061978578567505f, -0.1669226437807083f, -0.17805080115795135f, -0.17805080115795135f, -0.1965802013874054f, -0.20400193333625793f, -0.1965871900320053f, -0.2114201933145523f, -0.21142396330833435f, -0.21883843839168549f, -0.21884234249591827f, -0.2040202021598816f, -0.21512550115585327f, -0.2151293158531189f, -0.20771105587482452f, -0.20400193333625793f, -0.1891823709011078f, -0.1780412495136261f, -0.1780412495136261f, -0.16692562401294708f, -0.1557832956314087f, -0.15207688510417938f, -0.13724996149539948f, -0.13354049623012543f, -0.12982173264026642f, -0.11870266497135162f, -0.11128374934196472f, -0.1112857386469841f, -0.10385739058256149f, -0.0964476466178894f, -0.1743476688861847f, -0.1632249653339386f, -0.12241432070732117f, -0.055647868663072586f, 0.0037095914594829082f, 0.048224691301584244f, 0.1038760244846344f, 0.12613743543624878f, 0.14838366210460663f, 0.1854962259531021f, 0.20774829387664795f, 0.3079182207584381f, 0.30789607763290405f, 0.32646751403808594f, 0.32274025678634644f, 0.31161126494407654f, 0.30047690868377686f, 0.2633999288082123f, 0.25227487087249756f, 0.22257548570632935f, 0.21886590123176575f, 0.1706412136554718f, 0.1520959883928299f, 0.10387788712978363f, 0.07048982381820679f, 0.04080624133348465f, -0.007419448811560869f, -0.06307098269462585f, -0.09274811297655106f, -0.1335596740245819f, -0.20775577425956726f, -0.21888945996761322f, -0.21888555586338043f, -0.21516016125679016f, -0.2077445685863495f, -0.18919594585895538f, -0.07790981233119965f, -0.02226034551858902f, -0.15582521259784698f, -0.12985433638095856f, -0.1038946881890297f, -0.04452308639883995f, -0.0f, 0.05936839431524277f, 0.08163154125213623f, 0.11501797288656235f, 0.14098726212978363f, 0.1744009554386139f, 0.19665780663490295f, 0.2300318479537964f, 0.25602155923843384f, 0.26344725489616394f, 0.2857155501842499f, 0.30423566699028015f, 0.31165605783462524f, 0.2931050956249237f, 0.30796799063682556f, 0.29311034083366394f, 0.26715296506881714f, 0.25971338152885437f, 0.24487261474132538f, 0.2300359457731247f, 0.12985900044441223f, 0.07792241871356964f, 0.011130972765386105f, -0.05194360017776489f, -0.11130772531032562f, -0.16697360575199127f, -0.2152182012796402f, -0.24117109179496765f, -0.2560354471206665f, -0.2560123801231384f, -0.23746076226234436f, -0.23746076226234436f, -0.23006495833396912f, -0.15583361685276031f, -0.13728447258472443f, -0.11131174117326736f, 0.007421985734254122f, -0.014842634089291096f, -0.05937053635716438f, -0.11502832919359207f, -0.13729436695575714f, -0.1744009554386139f, -0.21151892840862274f, -0.2263542264699936f, -0.24861860275268555f, -0.2449122816324234f, -0.2449299395084381f, -0.2263583093881607f, -0.230069100856781f, -0.1781180202960968f, -0.1447208821773529f, -0.1001913845539093f, -0.08164625614881516f, -0.01855395920574665f, 0.01113237626850605f, 0.0519510880112648f, 0.0816374197602272f, 0.1187453418970108f, 0.1521424800157547f, 0.19296464323997498f, 0.285736083984375f, 0.3154173195362091f, 0.31541162729263306f, 0.31913381814956665f, 0.31544575095176697f, 0.3042849600315094f, 0.3006012737751007f, 0.24492110311985016f, 0.19668971002101898f, 0.17070259153842926f, 0.12246276438236237f, 0.09648581594228745f, 0.07421717047691345f, 0.02968686819076538f, -0.04082386568188667f, -0.18554963171482086f}; + +static constexpr float mag_data1_z[231] = {-0.40708473324775696f, -0.3992324769496918f, -0.38762804865837097f, -0.37180647253990173f, -0.35184505581855774f, -0.34016337990760803f, -0.3085607588291168f, -0.29266035556793213f, -0.29262199997901917f, -0.264837384223938f, -0.2806485593318939f, -0.2490914911031723f, -0.25314486026763916f, -0.25301215052604675f, -0.24523405730724335f, -0.2451055347919464f, -0.23342911899089813f, -0.24922214448451996f, -0.24902619421482086f, -0.26095297932624817f, -0.27277871966362f, -0.30444514751434326f, -0.308398962020874f, -0.3320784568786621f, -0.35212188959121704f, -0.35584497451782227f, -0.3798168897628784f, -0.3955906927585602f, -0.40724480152130127f, -0.42300471663475037f, -0.4230601489543915f, -0.44282928109169006f, -0.4388754665851593f, -0.4665523171424866f, -0.46253785490989685f, -0.454988956451416f, -0.803681492805481f, -0.830958366394043f, -0.8627271056175232f, -0.879018247127533f, -0.8706420660018921f, -0.8630667924880981f, -0.855260968208313f, -0.834915280342102f, -0.7994077205657959f, -0.7443938255310059f, -0.6692503690719604f, -0.6292367577552795f, -0.5777897238731384f, -0.5461300015449524f, -0.4907255470752716f, -0.4236709475517273f, -0.36031830310821533f, -0.3128037750720978f, -0.30076727271080017f, -0.30100417137145996f, -0.32055458426475525f, -0.32468241453170776f, -0.3482111394405365f, -0.36437347531318665f, -0.3759097158908844f, -0.42350417375564575f, -0.5028617978096008f, -0.5343277454376221f, -0.5817471742630005f, -0.6059688925743103f, -0.6688991189002991f, -0.688779354095459f, -0.7445892691612244f, -0.8311764597892761f, -0.8470083475112915f, -0.8591077923774719f, -0.8595589399337769f, -0.8787875175476074f, -0.3330386281013489f, -0.38061556220054626f, -0.43193021416664124f, -0.4675327241420746f, -0.5035226941108704f, -0.5549914240837097f, -0.5824348330497742f, -0.6340259909629822f, -0.6538393497467041f, -0.6815779805183411f, -0.7176189422607422f, -0.7453721165657043f, -0.765195906162262f, -0.7963905334472656f, -0.7925325036048889f, -0.8084893822669983f, -0.8202711343765259f, -0.820378839969635f, -0.8281964659690857f, -0.8362315893173218f, -0.8208101391792297f, -0.8201633095741272f, -0.8083831667900085f, -0.7925325036048889f, -0.74498051404953f, -0.6978869438171387f, -0.6777042746543884f, -0.6380724310874939f, -0.6106510162353516f, -0.5864740610122681f, -0.5588088035583496f, -0.5353109836578369f, -0.47979724407196045f, -0.45576605200767517f, -0.4004919230937958f, -0.37273505330085754f, -0.34105777740478516f, -0.3249810039997101f, -0.29743412137031555f, -0.392613023519516f, -0.36494842171669006f, -0.34105777740478516f, -0.2976297438144684f, -0.2736753523349762f, -0.2657427489757538f, -0.26588255167007446f, -0.2778243124485016f, -0.2974731922149658f, -0.3095756471157074f, -0.3293335735797882f, -0.4722391664981842f, -0.527519166469574f, -0.5555754899978638f, -0.6029582619667053f, -0.6664276123046875f, -0.6861714720726013f, -0.742090106010437f, -0.766001284122467f, -0.7813629508018494f, -0.8091270923614502f, -0.8249923586845398f, -0.8489018082618713f, -0.8453795909881592f, -0.8534297943115234f, -0.8608022928237915f, -0.8410787582397461f, -0.8098725080490112f, -0.7779080271720886f, -0.7581631541252136f, -0.6350269317626953f, -0.619232714176178f, -0.5556486248970032f, -0.5275884866714478f, -0.49195173382759094f, -0.43640878796577454f, -0.30167749524116516f, -0.2818673849105835f, -0.7861561179161072f, -0.790126621723175f, -0.7986983060836792f, -0.8062208294868469f, -0.8218905329704285f, -0.8384345769882202f, -0.8463818430900574f, -0.845936119556427f, -0.8577378392219543f, -0.8664782643318176f, -0.858302652835846f, -0.8656798601150513f, -0.8581897020339966f, -0.858302652835846f, -0.8663641214370728f, -0.877592921257019f, -0.8696508407592773f, -0.8617088794708252f, -0.8581897020339966f, -0.8578507304191589f, -0.8542165756225586f, -0.8339117765426636f, -0.8339117765426636f, -0.8300500512123108f, -0.8181354403495789f, -0.8067519068717957f, -0.7824946641921997f, -0.7625340223312378f, -0.7506194114685059f, -0.7629356980323792f, -0.7512127757072449f, -0.7427741289138794f, -0.747238039970398f, -0.7546902298927307f, -0.7507182359695435f, -0.7626343369483948f, -0.7672125101089478f, -0.8063269853591919f, -0.8104057908058167f, -0.8183509111404419f, -0.3221605718135834f, -0.3139989972114563f, -0.34182170033454895f, -0.36562156677246094f, -0.373619019985199f, -0.4173404276371002f, -0.4731728732585907f, -0.548576831817627f, -0.5724279880523682f, -0.6241878271102905f, -0.6523617506027222f, -0.6997264623641968f, -0.7315322160720825f, -0.7991194128990173f, -0.8508037328720093f, -0.8786337971687317f, -0.8952445983886719f, -0.9064638614654541f, -0.9263424277305603f, -0.9263424277305603f, -0.9342938661575317f, -0.9104394912719727f, -0.9144152402877808f, -0.8747733235359192f, -0.7833197712898254f, -0.7355079650878906f, -0.6996342539787292f, -0.6282463073730469f, -0.5689026713371277f, -0.5486491918563843f, -0.49331414699554443f, -0.42153728008270264f, -0.37396395206451416f, -0.36586251854896545f, -0.32613787055015564f, -0.3301151692867279f, -0.31809940934181213f, -0.32207563519477844f, -0.3223305940628052f, -0.43750202655792236f}; diff --git a/src/prometheus_px4/src/modules/commander/px4_custom_mode.h b/src/prometheus_px4/src/modules/commander/px4_custom_mode.h new file mode 100644 index 0000000..75a7bb0 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/px4_custom_mode.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 + +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_ */ diff --git a/src/prometheus_px4/src/modules/commander/rc_calibration.cpp b/src/prometheus_px4/src/modules/commander/rc_calibration.cpp new file mode 100644 index 0000000..c7d4561 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/rc_calibration.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#include "rc_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include + +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; +} diff --git a/src/prometheus_px4/src/modules/commander/rc_calibration.h b/src/prometheus_px4/src/modules/commander/rc_calibration.h new file mode 100644 index 0000000..fef1a43 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/rc_calibration.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 +#include + +int do_trim_calibration(orb_advert_t *mavlink_log_pub); + +#endif /* RC_CALIBRATION_H_ */ diff --git a/src/prometheus_px4/src/modules/commander/state_machine_helper.cpp b/src/prometheus_px4/src/modules/commander/state_machine_helper.cpp new file mode 100644 index 0000000..6cadb0d --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/state_machine_helper.cpp @@ -0,0 +1,1173 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 state_machine_helper.cpp + * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Sander Smeets + */ +#include +#include +#include +#include +#include +#include +#include + +#include "state_machine_helper.h" +#include "commander_helper.h" + +using namespace time_literals; + +static constexpr const char reason_no_rc[] = "No manual control stick input"; +static constexpr const char reason_no_offboard[] = "no offboard"; +static constexpr const char reason_no_rc_and_no_offboard[] = "no RC and no offboard"; +static constexpr const char reason_no_local_position[] = "no local position"; +static constexpr const char reason_no_global_position[] = "no global position"; +static constexpr const char reason_no_datalink[] = "no datalink"; +static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink"; + +// This array defines the arming state transitions. The rows are the new state, and the columns +// are the current state. Using new state and current state you can index into the array which +// will be true for a valid transition or false for a invalid transition. In some cases even +// though the transition is marked as true additional checks must be made. See arming_state_transition +// code for those checks. +static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] += { + // INIT, STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_SHUTDOWN */ true, true, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI +}; + +// You can index into the array with an arming_state_t in order to get its textual representation +const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { + "INIT", + "STANDBY", + "ARMED", + "STANDBY_ERROR", + "SHUTDOWN", + "IN_AIR_RESTORE", +}; + +// You can index into the array with an navigation_state_t in order to get its textual representation +const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { + "MANUAL", + "ALTCTL", + "POSCTL", + "AUTO_MISSION", + "AUTO_LOITER", + "AUTO_RTL", + "6: unallocated", + "7: unallocated", + "AUTO_LANDENGFAIL", + "AUTO_LANDGPSFAIL", + "ACRO", + "11: UNUSED", + "DESCEND", + "TERMINATION", + "OFFBOARD", + "STAB", + "16: UNUSED2", + "AUTO_TAKEOFF", + "AUTO_LAND", + "AUTO_FOLLOW_TARGET", + "AUTO_PRECLAND", + "ORBIT" +}; + +static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately + +void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, + const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act, + const float ll_delay); + +void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, const link_loss_actions_t link_loss_act); + +void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, + const vehicle_status_flags_s &status_flags, + const offboard_loss_actions_t offboard_loss_act); + +void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed, + const vehicle_status_flags_s &status_flags, + const offboard_loss_rc_actions_t offboard_loss_rc_act); + +void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsafe, + const offboard_loss_actions_t offboard_loss_act, + const offboard_loss_rc_actions_t offboard_loss_rc_act); + +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) +{ + // Double check that our static arrays are still valid + static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); + static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1, + "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1"); + + transition_result_t ret = TRANSITION_DENIED; + arming_state_t current_arming_state = status.arming_state; + bool feedback_provided = false; + + const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON); + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_arming_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + /* + * Get sensing state if necessary + */ + bool preflight_check_ret = true; + + /* only perform the pre-arm check if we have to */ + if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) + && !hil_enabled) { + + preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, true, true, + time_since_boot); + + if (preflight_check_ret) { + status_flags.condition_system_sensors_initialized = true; + } + + feedback_provided = true; + } + + /* re-run the pre-flight check as long as sensors are failing */ + if (!status_flags.condition_system_sensors_initialized + && fRunPreArmChecks + && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) + || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) + && !hil_enabled) { + + if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) { + + status_flags.condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status, + status_flags, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED, + time_since_boot); + + last_preflight_check = hrt_absolute_time(); + } + } + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status.arming_state]; + + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + // Do not perform pre-arm checks if coming from in air restore + // Allow if vehicle_status_s::HIL_STATE_ON + if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) { + + bool prearm_check_ret = true; + + if (fRunPreArmChecks && preflight_check_ret) { + // only bother running prearm if preflight was successful + prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, safety, arm_requirements, status); + } + + if (!preflight_check_ret || !prearm_check_ret) { + // the prearm and preflight checks already print the rejection reason + feedback_provided = true; + valid_transition = false; + } + } + } + } + + if (hil_enabled) { + /* enforce lockdown in HIL */ + armed.lockdown = true; + status_flags.condition_system_sensors_initialized = true; + + /* recover from a prearm fail */ + if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { + status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY; + } + + // HIL can always go to standby + if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + valid_transition = true; + } + } + + if (!hil_enabled && + (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { + + // Sensors need to be initialized for STANDBY state, except for HIL + if (!status_flags.condition_system_sensors_initialized) { + feedback_provided = true; + valid_transition = false; + } + } + + // Finish up the state transition + if (valid_transition) { + bool was_armed = armed.armed; + armed.armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED); + armed.ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) + || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY); + ret = TRANSITION_CHANGED; + status.arming_state = new_arming_state; + + if (was_armed && !armed.armed) { // disarm transition + status.latest_disarming_reason = (uint8_t)calling_reason; + + } else if (!was_armed && armed.armed) { // arm transition + status.latest_arming_reason = (uint8_t)calling_reason; + } + + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + status.armed_time = hrt_absolute_time(); + + } else { + status.armed_time = 0; + } + } + } + + if (ret == TRANSITION_DENIED) { + /* print to MAVLink and console if we didn't provide any feedback yet */ + if (!feedback_provided) { + mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s", + arming_state_names[status.arming_state], arming_state_names[new_arming_state]); + } + } + + return ret; +} + +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) +{ + // IMPORTANT: The assumption of callers of this function is that the execution of + // this check is essentially "free". Therefore any runtime checking in here has to be + // kept super lightweight. No complex logic or calls on external function should be + // implemented here. + + transition_result_t ret = TRANSITION_DENIED; + + /* transition may be denied even if the same state is requested because conditions may have changed */ + switch (new_main_state) { + case commander_state_s::MAIN_STATE_MANUAL: + case commander_state_s::MAIN_STATE_STAB: + case commander_state_s::MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + + case commander_state_s::MAIN_STATE_ALTCTL: + + /* need at minimum altitude estimate */ + if (status_flags.condition_local_altitude_valid || + status_flags.condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_POSCTL: + + /* need at minimum local position estimate */ + if (status_flags.condition_local_position_valid || + status_flags.condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_LOITER: + + /* need global position estimate */ + if (status_flags.condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: + case commander_state_s::MAIN_STATE_ORBIT: + + /* Follow and orbit only implemented for multicopter */ + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_MISSION: + + /* need global position, home position, and a valid mission */ + if (status_flags.condition_global_position_valid && + status_flags.condition_auto_mission_available) { + + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_RTL: + + /* need global position and home position */ + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: + case commander_state_s::MAIN_STATE_AUTO_LAND: + + /* need local position */ + if (status_flags.condition_local_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_PRECLAND: + + /* need local and global position, and precision land only implemented for multicopters */ + if (status_flags.condition_local_position_valid + && status_flags.condition_global_position_valid + && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_OFFBOARD: + + /* need offboard signal */ + if (!status_flags.offboard_control_signal_lost) { + + ret = TRANSITION_CHANGED; + } + + break; + + case commander_state_s::MAIN_STATE_MAX: + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + if (internal_state.main_state != new_main_state) { + internal_state.main_state = new_main_state; + internal_state.main_state_changes++; + internal_state.timestamp = hrt_absolute_time(); + + } else { + ret = TRANSITION_NOT_CHANGED; + } + } + + return ret; +} + +/** + * Enable failsafe and report to user + */ +void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason) +{ + if (!old_failsafe && status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + // make sure intermittent failsafes don't lead to infinite delay by not constantly reseting the timestamp + if (status.failsafe_timestamp == 0 || + hrt_elapsed_time(&status.failsafe_timestamp) > 30_s) { + status.failsafe_timestamp = hrt_absolute_time(); + } + + mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason); + } + + status.failsafe = true; +} + +/** + * Check failsafe and main status and set navigation status for navigator accordingly + */ +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) +{ + const navigation_state_t nav_state_old = status.nav_state; + + const bool is_armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + const bool data_link_loss_act_configured = (data_link_loss_act > link_loss_actions_t::DISABLED); + + bool old_failsafe = status.failsafe; + status.failsafe = false; + + // Safe to do reset flags here, as if loss state persists flags will be restored in the code below + reset_link_loss_globals(armed, old_failsafe, rc_loss_act); + reset_link_loss_globals(armed, old_failsafe, data_link_loss_act); + reset_offboard_loss_globals(armed, old_failsafe, offb_loss_act, offb_loss_rc_act); + + // Failsafe decision logic for every normal non-failsafe mode + switch (internal_state.main_state) { + case commander_state_s::MAIN_STATE_ACRO: + case commander_state_s::MAIN_STATE_MANUAL: + case commander_state_s::MAIN_STATE_STAB: + case commander_state_s::MAIN_STATE_ALTCTL: + + // Require RC for all manual modes + if (status.rc_signal_lost && is_armed) { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); + + } else { + switch (internal_state.main_state) { + case commander_state_s::MAIN_STATE_ACRO: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; + break; + + case commander_state_s::MAIN_STATE_MANUAL: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + break; + + case commander_state_s::MAIN_STATE_STAB: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; + break; + + case commander_state_s::MAIN_STATE_ALTCTL: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + break; + + default: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + break; + } + } + + break; + + case commander_state_s::MAIN_STATE_POSCTL: { + + const bool rc_fallback_allowed = (posctl_nav_loss_act != position_nav_loss_actions_t::LAND_TERMINATE) || !is_armed; + + if (status.rc_signal_lost && is_armed) { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); + + /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ + /* A local position estimate is enough for POSCTL for multirotors, + * this enables POSCTL using e.g. flow. + * For fixedwing, a global position is needed. */ + + } else if (check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, + rc_fallback_allowed, status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + // nothing to do - everything done in check_invalid_pos_nav_state + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + } + } + break; + + case commander_state_s::MAIN_STATE_AUTO_MISSION: + + /* go into failsafe + * - if we have an engine failure + * - if we have vtol transition failure + * - on data and RC link loss */ + + if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { + // nothing to do - everything done in check_invalid_pos_nav_state + } else if (status.engine_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (status_flags.vtol_transition_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + } else if (status.mission_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + } else if (status.data_link_lost && data_link_loss_act_configured + && is_armed && !landed) { + // Data link lost, data link loss reaction configured -> do configured reaction + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); + + } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) + && status_flags.rc_signal_found_once && is_armed && !landed) { + // RC link lost, rc loss not disabled in mission, RC was used before -> RC loss reaction after delay + // Safety pilot expects to be able to take over by RC in case anything unexpected happens + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); + + } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) + && status.data_link_lost && !data_link_loss_act_configured + && is_armed && !landed) { + // All links lost, no data link loss reaction configured -> immediately do RC loss reaction + // Lost all communication, by default it's considered unsafe to continue the mission + // This is only reached when flying mission completely without RC (it was not present since boot) + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); + + } else if (status.rc_signal_lost && (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION) + && status.data_link_lost && !data_link_loss_act_configured + && is_armed && !landed + && mission_finished) { + // All links lost, all link loss reactions disabled -> return after mission finished + // Pilot disabled all reactions, finish mission but then return to avoid lost vehicle + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_datalink); + set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0); + + } else if (!stay_in_failsafe) { + // normal mission operation if there's no need to stay in failsafe + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_LOITER: + + /* go into failsafe on a engine failure */ + if (status.engine_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { + // nothing to do - everything done in check_invalid_pos_nav_state + } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { + // Data link lost, data link loss reaction configured -> do configured reaction + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); + + } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) + && status_flags.rc_signal_found_once && is_armed && !landed) { + // RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay + // Safety pilot expects to be able to take over by RC in case anything unexpected happens + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); + + } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) + && status.data_link_lost && !data_link_loss_act_configured + && is_armed && !landed) { + // All links lost, no data link loss reaction configured -> immediately do RC loss reaction + // Lost all communication, by default it's considered unsafe to continue the mission + // This is only reached when flying mission completely without RC (it was not present since boot) + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_RTL: + + /* require global position and home, also go into failsafe on an engine failure */ + + if (status.engine_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { + // nothing to do - everything done in check_invalid_pos_nav_state + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: + + /* require global position and home */ + + if (status.engine_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { + // nothing to do - everything done in check_invalid_pos_nav_state + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET; + } + + break; + + case commander_state_s::MAIN_STATE_ORBIT: + if (status.engine_failure) { + // failsafe: on engine failure + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + // Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; + + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { + // failsafe: necessary position estimate lost; switching is done in check_invalid_pos_nav_state + + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; + + } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { + // failsafe: just datalink is lost and we're in air + set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); + + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; + + } else if (status.rc_signal_lost && status.data_link_lost && !data_link_loss_act_configured && is_armed) { + // Orbit does not depend on RC but while armed & all links lost & when datalink loss is not set up, we failsafe + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); + + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; + + } else { + // no failsafe, RC is not mandatory for orbit + status.nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: + + /* require local position */ + + if (status.engine_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { + // nothing to do - everything done in check_invalid_pos_nav_state + } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { + // Data link lost, data link loss reaction configured -> do configured reaction + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0); + + } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) + && status_flags.rc_signal_found_once && is_armed && !landed) { + // RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay + // Safety pilot expects to be able to take over by RC in case anything unexpected happens + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); + + } else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) + && status.data_link_lost && !data_link_loss_act_configured + && is_armed && !landed) { + // All links lost, no data link loss reaction configured -> immediately do RC loss reaction + // Lost all communication, by default it's considered unsafe to continue the mission + // This is only reached when flying mission completely without RC (it was not present since boot) + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_LAND: + + /* require local position */ + + if (status.engine_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { + // nothing to do - everything done in check_invalid_pos_nav_state + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + } + + break; + + case commander_state_s::MAIN_STATE_AUTO_PRECLAND: + + /* must be rotary wing plus same requirements as normal landing */ + + if (status.engine_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { + // nothing to do - everything done in check_invalid_pos_nav_state + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; + } + + break; + + case commander_state_s::MAIN_STATE_OFFBOARD: + + if (status_flags.offboard_control_signal_lost) { + if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD)) { + // Offboard and RC are lost + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard); + set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act); + + } else { + // Offboard is lost, RC is ok + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard); + set_offboard_loss_rc_nav_state(status, armed, status_flags, offb_loss_rc_act); + } + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + } + + default: + break; + } + + return status.nav_state != nav_state_old; +} + +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) +{ + bool fallback_required = false; + + if (using_global_pos && !status_flags.condition_global_position_valid) { + fallback_required = true; + + } else if (!using_global_pos + && (!status_flags.condition_local_position_valid || !status_flags.condition_local_velocity_valid)) { + + fallback_required = true; + } + + if (fallback_required) { + if (use_rc) { + // fallback to a mode that gives the operator stick control + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && status_flags.condition_local_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + + } else if (status_flags.condition_local_altitude_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; + } + + } else { + // go into a descent that does not require stick control + if (status_flags.condition_local_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + + } else if (status_flags.condition_local_altitude_valid) { + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + // TODO: FW position controller doesn't run without condition_global_position_valid + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } + + if (using_global_pos) { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_global_position); + + } else { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position); + } + + } + + return fallback_required; + +} + +void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, + const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act, + const float ll_delay) +{ + if (hrt_elapsed_time(&status.failsafe_timestamp) < (ll_delay * 1_s) + && link_loss_act != link_loss_actions_t::DISABLED) { + // delay failsafe reaction by trying to hold position if configured + link_loss_act = link_loss_actions_t::AUTO_LOITER; + } + + // do the best you can according to the action set + switch (link_loss_act) { + case link_loss_actions_t::DISABLED: + // If datalink loss failsafe is disabled then no action must be taken. + break; + + case link_loss_actions_t::AUTO_RTL: + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state); + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + return; + } + + // FALLTHROUGH + case link_loss_actions_t::AUTO_LOITER: + if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + return; + } + + // FALLTHROUGH + case link_loss_actions_t::AUTO_LAND: + if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + return; + + } else { + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (status_flags.condition_local_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + return; + + } else if (status_flags.condition_local_altitude_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + return; + } + + } else { + // TODO: FW position controller doesn't run without condition_global_position_valid + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + return; + } + } + + // FALLTHROUGH + case link_loss_actions_t::TERMINATE: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + armed.force_failsafe = true; + break; + + case link_loss_actions_t::LOCKDOWN: + armed.lockdown = true; + break; + } +} + +void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, const link_loss_actions_t link_loss_act) +{ + if (old_failsafe) { + if (link_loss_act == link_loss_actions_t::TERMINATE) { + armed.force_failsafe = false; + + } else if (link_loss_act == link_loss_actions_t::LOCKDOWN) { + armed.lockdown = false; + } + } +} + +void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, + const vehicle_status_flags_s &status_flags, + const offboard_loss_actions_t offboard_loss_act) +{ + switch (offboard_loss_act) { + case offboard_loss_actions_t::DISABLED: + // If offboard loss failsafe is disabled then no action must be taken. + return; + + case offboard_loss_actions_t::TERMINATE: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + armed.force_failsafe = true; + return; + + case offboard_loss_actions_t::LOCKDOWN: + armed.lockdown = true; + return; + + case offboard_loss_actions_t::AUTO_RTL: + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + return; + } + + // FALLTHROUGH + case offboard_loss_actions_t::AUTO_LOITER: + if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + return; + } + + // FALLTHROUGH + case offboard_loss_actions_t::AUTO_LAND: + if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + return; + } + } + + // If none of the above worked, try to mitigate + if (status_flags.condition_local_altitude_valid) { + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + // TODO: FW position controller doesn't run without condition_global_position_valid + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } +} + +void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed, + const vehicle_status_flags_s &status_flags, + const offboard_loss_rc_actions_t offboard_loss_rc_act) +{ + switch (offboard_loss_rc_act) { + case offboard_loss_rc_actions_t::DISABLED: + // If offboard loss failsafe is disabled then no action must be taken. + return; + + case offboard_loss_rc_actions_t::TERMINATE: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + armed.force_failsafe = true; + return; + + case offboard_loss_rc_actions_t::LOCKDOWN: + armed.lockdown = true; + return; + + case offboard_loss_rc_actions_t::MANUAL_POSITION: + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && status_flags.condition_local_position_valid) { + + status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + return; + + } else if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + return; + } + + // FALLTHROUGH + case offboard_loss_rc_actions_t::MANUAL_ALTITUDE: + if (status_flags.condition_local_altitude_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + return; + } + + // FALLTHROUGH + case offboard_loss_rc_actions_t::MANUAL_ATTITUDE: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + return; + + case offboard_loss_rc_actions_t::AUTO_RTL: + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + return; + } + + // FALLTHROUGH + case offboard_loss_rc_actions_t::AUTO_LOITER: + if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + return; + } + + // FALLTHROUGH + case offboard_loss_rc_actions_t::AUTO_LAND: + if (status_flags.condition_global_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + return; + } + } + + // If none of the above worked, try to mitigate + if (status_flags.condition_local_altitude_valid) { + //TODO: Add case for rover + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + // TODO: FW position controller doesn't run without condition_global_position_valid + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } + + } else { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } +} + +void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsafe, + const offboard_loss_actions_t offboard_loss_act, + const offboard_loss_rc_actions_t offboard_loss_rc_act) +{ + if (old_failsafe) { + if (offboard_loss_act == offboard_loss_actions_t::TERMINATE + || offboard_loss_rc_act == offboard_loss_rc_actions_t::TERMINATE) { + armed.force_failsafe = false; + + } + + if (offboard_loss_act == offboard_loss_actions_t::LOCKDOWN + || offboard_loss_rc_act == offboard_loss_rc_actions_t::LOCKDOWN) { + armed.lockdown = false; + } + } +} + +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_battery_action) +{ + switch (battery_warning) { + case battery_status_s::BATTERY_WARNING_NONE: + break; + + case battery_status_s::BATTERY_WARNING_LOW: + mavlink_log_critical(mavlink_log_pub, "Low battery level! Return advised"); + break; + + case battery_status_s::BATTERY_WARNING_CRITICAL: + + static constexpr char battery_critical[] = "Critical battery level!"; + + switch (low_battery_action) { + case LOW_BAT_ACTION::WARNING: + mavlink_log_critical(mavlink_log_pub, "%s, landing advised", battery_critical); + break; + + case LOW_BAT_ACTION::RETURN: + + // FALLTHROUGH + case LOW_BAT_ACTION::RETURN_OR_LAND: + + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + + internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; + internal_state.timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_critical); + } + + } else { + if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + internal_state.timestamp = hrt_absolute_time(); + mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_critical); + } + } + + break; + + case LOW_BAT_ACTION::LAND: + if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + internal_state.timestamp = hrt_absolute_time(); + mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_critical); + } + + break; + } + + break; + + case battery_status_s::BATTERY_WARNING_EMERGENCY: + + static constexpr char battery_dangerous[] = "Dangerous battery level!"; + + switch (low_battery_action) { + case LOW_BAT_ACTION::WARNING: + mavlink_log_emergency(mavlink_log_pub, "%s, please land!", battery_dangerous); + break; + + case LOW_BAT_ACTION::RETURN: + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL; + internal_state.timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_log_pub, "%s, executing RTL", battery_dangerous); + } + + } else { + if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + internal_state.timestamp = hrt_absolute_time(); + mavlink_log_emergency(mavlink_log_pub, "%s, can't execute RTL, landing instead", battery_dangerous); + } + } + + break; + + case LOW_BAT_ACTION::RETURN_OR_LAND: + + // FALLTHROUGH + case LOW_BAT_ACTION::LAND: + if (!(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { + internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND; + internal_state.timestamp = hrt_absolute_time(); + mavlink_log_emergency(mavlink_log_pub, "%s, landing", battery_dangerous); + } + + break; + } + + break; + + case battery_status_s::BATTERY_WARNING_FAILED: + mavlink_log_emergency(mavlink_log_pub, "Battery failure detected"); + break; + } +} diff --git a/src/prometheus_px4/src/modules/commander/state_machine_helper.h b/src/prometheus_px4/src/modules/commander/state_machine_helper.h new file mode 100644 index 0000000..f5ce524 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/state_machine_helper.h @@ -0,0 +1,164 @@ +/**************************************************************************** + * + * 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 + * @author Julian Oes + */ + +#ifndef STATE_MACHINE_HELPER_H_ +#define STATE_MACHINE_HELPER_H_ + +#include + +#include "Arming/PreFlightCheck/PreFlightCheck.hpp" + +#include +#include +#include +#include +#include +#include +#include + +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_ */ diff --git a/src/prometheus_px4/src/modules/commander/worker_thread.cpp b/src/prometheus_px4/src/modules/commander/worker_thread.cpp new file mode 100644 index 0000000..e65732d --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/worker_thread.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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 +#include + +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, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + pthread_attr_setschedparam(&low_prio_attr, ¶m); +#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; +} diff --git a/src/prometheus_px4/src/modules/commander/worker_thread.hpp b/src/prometheus_px4/src/modules/commander/worker_thread.hpp new file mode 100644 index 0000000..df77a99 --- /dev/null +++ b/src/prometheus_px4/src/modules/commander/worker_thread.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include + +/** + * @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; + +}; + diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp new file mode 100644 index 0000000..bd6ea7a --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include + +#include +#include + +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 &matrix) = 0; + + /** + * Get the actuator trims + * + * @return Actuator trims + */ + const matrix::Vector &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 _trim; ///< Actuator trim + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase +}; diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp new file mode 100644 index 0000000..11cab60 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -0,0 +1,200 @@ +/**************************************************************************** + * + * 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 + */ + +#include "ActuatorEffectivenessMultirotor.hpp" + +ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(): + ModuleParams(nullptr) +{ +} + +bool +ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix &matrix) +{ + // Check if parameters have changed + if (_updated || _parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_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 &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; +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp new file mode 100644 index 0000000..2997f93 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include +#include +#include +#include + +using namespace time_literals; + +class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessMultirotor(); + virtual ~ActuatorEffectivenessMultirotor() = default; + + static constexpr int NUM_ROTORS_MAX = 8; + + typedef struct { + float position_x; + float position_y; + float position_z; + float axis_x; + float axis_y; + float axis_z; + float thrust_coef; + float moment_ratio; + } RotorGeometry; + + typedef struct { + RotorGeometry rotors[NUM_ROTORS_MAX]; + } MultirotorGeometry; + + static int computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness); + + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + + int numActuators() const override { return _num_actuators; } +private: + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + bool _updated{true}; + int _num_actuators{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_mc_r0_px, + (ParamFloat) _param_ca_mc_r0_py, + (ParamFloat) _param_ca_mc_r0_pz, + (ParamFloat) _param_ca_mc_r0_ax, + (ParamFloat) _param_ca_mc_r0_ay, + (ParamFloat) _param_ca_mc_r0_az, + (ParamFloat) _param_ca_mc_r0_ct, + (ParamFloat) _param_ca_mc_r0_km, + + (ParamFloat) _param_ca_mc_r1_px, + (ParamFloat) _param_ca_mc_r1_py, + (ParamFloat) _param_ca_mc_r1_pz, + (ParamFloat) _param_ca_mc_r1_ax, + (ParamFloat) _param_ca_mc_r1_ay, + (ParamFloat) _param_ca_mc_r1_az, + (ParamFloat) _param_ca_mc_r1_ct, + (ParamFloat) _param_ca_mc_r1_km, + + (ParamFloat) _param_ca_mc_r2_px, + (ParamFloat) _param_ca_mc_r2_py, + (ParamFloat) _param_ca_mc_r2_pz, + (ParamFloat) _param_ca_mc_r2_ax, + (ParamFloat) _param_ca_mc_r2_ay, + (ParamFloat) _param_ca_mc_r2_az, + (ParamFloat) _param_ca_mc_r2_ct, + (ParamFloat) _param_ca_mc_r2_km, + + (ParamFloat) _param_ca_mc_r3_px, + (ParamFloat) _param_ca_mc_r3_py, + (ParamFloat) _param_ca_mc_r3_pz, + (ParamFloat) _param_ca_mc_r3_ax, + (ParamFloat) _param_ca_mc_r3_ay, + (ParamFloat) _param_ca_mc_r3_az, + (ParamFloat) _param_ca_mc_r3_ct, + (ParamFloat) _param_ca_mc_r3_km, + + (ParamFloat) _param_ca_mc_r4_px, + (ParamFloat) _param_ca_mc_r4_py, + (ParamFloat) _param_ca_mc_r4_pz, + (ParamFloat) _param_ca_mc_r4_ax, + (ParamFloat) _param_ca_mc_r4_ay, + (ParamFloat) _param_ca_mc_r4_az, + (ParamFloat) _param_ca_mc_r4_ct, + (ParamFloat) _param_ca_mc_r4_km, + + (ParamFloat) _param_ca_mc_r5_px, + (ParamFloat) _param_ca_mc_r5_py, + (ParamFloat) _param_ca_mc_r5_pz, + (ParamFloat) _param_ca_mc_r5_ax, + (ParamFloat) _param_ca_mc_r5_ay, + (ParamFloat) _param_ca_mc_r5_az, + (ParamFloat) _param_ca_mc_r5_ct, + (ParamFloat) _param_ca_mc_r5_km, + + (ParamFloat) _param_ca_mc_r6_px, + (ParamFloat) _param_ca_mc_r6_py, + (ParamFloat) _param_ca_mc_r6_pz, + (ParamFloat) _param_ca_mc_r6_ax, + (ParamFloat) _param_ca_mc_r6_ay, + (ParamFloat) _param_ca_mc_r6_az, + (ParamFloat) _param_ca_mc_r6_ct, + (ParamFloat) _param_ca_mc_r6_km, + + (ParamFloat) _param_ca_mc_r7_px, + (ParamFloat) _param_ca_mc_r7_py, + (ParamFloat) _param_ca_mc_r7_pz, + (ParamFloat) _param_ca_mc_r7_ax, + (ParamFloat) _param_ca_mc_r7_ay, + (ParamFloat) _param_ca_mc_r7_az, + (ParamFloat) _param_ca_mc_r7_ct, + (ParamFloat) _param_ca_mc_r7_km + ) +}; diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c new file mode 100644 index 0000000..2fea6ea --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c @@ -0,0 +1,560 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessMultirotorParams.c + * + * Parameters for the actuator effectiveness of multirotors. + * + * @author Julien Lecoeur + */ + +/** + * Position of rotor 0 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_PX, 0.0); + +/** + * Position of rotor 0 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_PY, 0.0); + +/** + * Position of rotor 0 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_PZ, 0.0); + +/** + * Axis of rotor 0 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_AX, 0.0); + +/** + * Axis of rotor 0 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_AY, 0.0); + +/** + * Axis of rotor 0 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_AZ, -1.0); + +/** + * Thrust coefficient of rotor 0 + * + * The thrust coefficient if defined as Thrust = CT * u^2, + * where u (with value between CA_ACT0_MIN and CA_ACT0_MAX) + * is the output signal sent to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_CT, 0.0); + +/** + * Moment coefficient of rotor 0 + * + * The moment coefficient if defined as Torque = KM * Thrust + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_KM, 0.05); + +/** + * Position of rotor 1 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_PX, 0.0); + +/** + * Position of rotor 1 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_PY, 0.0); + +/** + * Position of rotor 1 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_PZ, 0.0); + +/** + * Axis of rotor 1 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_AX, 0.0); + +/** + * Axis of rotor 1 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_AY, 0.0); + +/** + * Axis of rotor 1 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_AZ, -1.0); + +/** + * Thrust coefficient of rotor 1 + * + * The thrust coefficient if defined as Thrust = CT * u^2, + * where u (with value between CA_ACT1_MIN and CA_ACT1_MAX) + * is the output signal sent to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_CT, 0.0); + +/** + * Moment coefficient of rotor 1 + * + * The moment coefficient if defined as Torque = KM * Thrust, + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_KM, 0.05); + +/** + * Position of rotor 2 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_PX, 0.0); + +/** + * Position of rotor 2 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_PY, 0.0); + +/** + * Position of rotor 2 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_PZ, 0.0); + +/** + * Axis of rotor 2 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_AX, 0.0); + +/** + * Axis of rotor 2 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_AY, 0.0); + +/** + * Axis of rotor 2 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_AZ, -1.0); + +/** + * Thrust coefficient of rotor 2 + * + * The thrust coefficient if defined as Thrust = CT * u^2, + * where u (with value between CA_ACT2_MIN and CA_ACT2_MAX) + * is the output signal sent to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_CT, 0.0); + +/** + * Moment coefficient of rotor 2 + * + * The moment coefficient if defined as Torque = KM * Thrust + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_KM, 0.05); + +/** + * Position of rotor 3 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_PX, 0.0); + +/** + * Position of rotor 3 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_PY, 0.0); + +/** + * Position of rotor 3 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_PZ, 0.0); + +/** + * Axis of rotor 3 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_AX, 0.0); + +/** + * Axis of rotor 3 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_AY, 0.0); + +/** + * Axis of rotor 3 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_AZ, -1.0); + +/** + * Thrust coefficient of rotor 3 + * + * The thrust coefficient if defined as Thrust = CT * u^2, + * where u (with value between CA_ACT3_MIN and CA_ACT3_MAX) + * is the output signal sent to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_CT, 0.0); + +/** + * Moment coefficient of rotor 3 + * + * The moment coefficient if defined as Torque = KM * Thrust + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_KM, 0.05); + +/** + * Position of rotor 4 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_PX, 0.0); + +/** + * Position of rotor 4 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_PY, 0.0); + +/** + * Position of rotor 4 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_PZ, 0.0); + +/** + * Axis of rotor 4 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_AX, 0.0); + +/** + * Axis of rotor 4 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_AY, 0.0); + +/** + * Axis of rotor 4 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_AZ, -1.0); + +/** + * Thrust coefficient of rotor 4 + * + * The thrust coefficient if defined as Thrust = CT * u^2, + * where u (with value between CA_ACT4_MIN and CA_ACT4_MAX) + * is the output signal sent to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_CT, 0.0); + +/** + * Moment coefficient of rotor 4 + * + * The moment coefficient if defined as Torque = KM * Thrust + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_KM, 0.05); + +/** + * Position of rotor 5 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_PX, 0.0); + +/** + * Position of rotor 5 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_PY, 0.0); + +/** + * Position of rotor 5 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_PZ, 0.0); + +/** + * Axis of rotor 5 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_AX, 0.0); + +/** + * Axis of rotor 5 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_AY, 0.0); + +/** + * Axis of rotor 5 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_AZ, -1.0); + +/** + * Thrust coefficient of rotor 5 + * + * The thrust coefficient if defined as Thrust = CT * u^2, + * where u (with value between CA_ACT5_MIN and CA_ACT5_MAX) + * is the output signal sent to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_CT, 0.0); + +/** + * Moment coefficient of rotor 5 + * + * The moment coefficient if defined as Torque = KM * Thrust + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_KM, 0.05); + +/** + * Position of rotor 6 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_PX, 0.0); + +/** + * Position of rotor 6 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_PY, 0.0); + +/** + * Position of rotor 6 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_PZ, 0.0); + +/** + * Axis of rotor 6 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_AX, 0.0); + +/** + * Axis of rotor 6 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_AY, 0.0); + +/** + * Axis of rotor 6 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_AZ, -1.0); + +/** + * Thrust coefficient of rotor 6 + * + * The thrust coefficient if defined as Thrust = CT * u^2, + * where u (with value between CA_ACT6_MIN and CA_ACT6_MAX) + * is the output signal sent to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_CT, 0.0); + +/** + * Moment coefficient of rotor 6 + * + * The moment coefficient if defined as Torque = KM * Thrust + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_KM, 0.05); + +/** + * Position of rotor 7 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_PX, 0.0); + +/** + * Position of rotor 7 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_PY, 0.0); + +/** + * Position of rotor 7 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_PZ, 0.0); + +/** + * Axis of rotor 7 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_AX, 0.0); + +/** + * Axis of rotor 7 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_AY, 0.0); + +/** + * Axis of rotor 7 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_AZ, -1.0); + +/** + * Thrust coefficient of rotor 7 + * + * The thrust coefficient if defined as Thrust = CT * u^2, + * where u (with value between CA_ACT7_MIN and CA_ACT7_MAX) + * is the output signal sent to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_CT, 0.0); + +/** + * Moment coefficient of rotor 7 + * + * The moment coefficient if defined as Torque = KM * Thrust + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_KM, 0.05); diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp new file mode 100644 index 0000000..69b60ca --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessMultirotorTest.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include + +using namespace matrix; + +TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase) +{ + // Quad wide geometry + ActuatorEffectivenessMultirotor::MultirotorGeometry geometry = {}; + geometry.rotors[0].position_x = 1.0f; + geometry.rotors[0].position_y = 1.0f; + geometry.rotors[0].position_z = 0.0f; + geometry.rotors[0].axis_x = 0.0f; + geometry.rotors[0].axis_y = 0.0f; + geometry.rotors[0].axis_z = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position_x = -1.0f; + geometry.rotors[1].position_y = -1.0f; + geometry.rotors[1].position_z = 0.0f; + geometry.rotors[1].axis_x = 0.0f; + geometry.rotors[1].axis_y = 0.0f; + geometry.rotors[1].axis_z = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position_x = 1.0f; + geometry.rotors[2].position_y = -1.0f; + geometry.rotors[2].position_z = 0.0f; + geometry.rotors[2].axis_x = 0.0f; + geometry.rotors[2].axis_y = 0.0f; + geometry.rotors[2].axis_z = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position_x = -1.0f; + geometry.rotors[3].position_y = 1.0f; + geometry.rotors[3].position_z = 0.0f; + geometry.rotors[3].axis_x = 0.0f; + geometry.rotors[3].axis_y = 0.0f; + geometry.rotors[3].axis_z = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + matrix::Matrix effectiveness = + ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry); + + const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { + {-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 1.0f, -1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + matrix::Matrix effectiveness_expected( + expected); + + EXPECT_EQ(effectiveness, effectiveness_expected); +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp new file mode 100644 index 0000000..c5b2b8b --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessStandardVTOL.hpp + * + * Actuator effectiveness for standard VTOL + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessStandardVTOL.hpp" + +ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL() +{ + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} + +bool +ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) +{ + if (!_updated) { + return false; + } + + switch (_flight_phase) { + case FlightPhase::HOVER_FLIGHT: { + const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + {-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + matrix = matrix::Matrix(standard_vtol); + break; + } + + case FlightPhase::FORWARD_FLIGHT: { + const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + { 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + matrix = matrix::Matrix(standard_vtol); + break; + } + + case FlightPhase::TRANSITION_HF_TO_FF: + case FlightPhase::TRANSITION_FF_TO_HF: { + const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + { -0.5f, 0.5f, 0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + matrix = matrix::Matrix(standard_vtol); + break; + } + } + + _updated = false; + return true; +} + +void +ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + _updated = true; + +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp new file mode 100644 index 0000000..135dc97 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessStandardVTOL.hpp + * + * Actuator effectiveness for standard VTOL + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessStandardVTOL(); + virtual ~ActuatorEffectivenessStandardVTOL() = default; + + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + void setFlightPhase(const FlightPhase &flight_phase) override; + + int numActuators() const override { return 7; } +protected: + bool _updated{true}; +}; diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp new file mode 100644 index 0000000..c200138 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessTiltrotorVTOL.hpp + * + * Actuator effectiveness for tiltrotor VTOL + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessTiltrotorVTOL.hpp" + +ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL() +{ + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} +bool +ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) +{ + if (!_updated) { + return false; + } + + // Trim + float tilt = 0.0f; + + switch (_flight_phase) { + case FlightPhase::HOVER_FLIGHT: { + tilt = 0.0f; + break; + } + + case FlightPhase::FORWARD_FLIGHT: { + tilt = 1.5f; + break; + } + + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: { + tilt = 1.0f; + break; + } + } + + // Trim: half throttle, tilted motors + _trim(0) = 0.5f; + _trim(1) = 0.5f; + _trim(2) = 0.5f; + _trim(3) = 0.5f; + _trim(4) = tilt; + _trim(5) = tilt; + _trim(6) = tilt; + _trim(7) = tilt; + + // Effectiveness + const float tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = { + {-0.5f * cosf(_trim(4)), 0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), 0.5f * _trim(0) *sinf(_trim(4)), -0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f * cosf(_trim(4)), -0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), -0.5f * _trim(0) *sinf(_trim(4)), 0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.5f * sinf(_trim(4)), 0.5f * sinf(_trim(5)), 0.5f * sinf(_trim(6)), -0.5f * sinf(_trim(7)), -0.5f * _trim(0) *cosf(_trim(4)), 0.5f * _trim(1) *cosf(_trim(5)), 0.5f * _trim(2) *cosf(_trim(6)), -0.5f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f * sinf(_trim(4)), 0.25f * sinf(_trim(5)), 0.25f * sinf(_trim(6)), 0.25f * sinf(_trim(7)), 0.25f * _trim(0) *cosf(_trim(4)), 0.25f * _trim(1) *cosf(_trim(5)), 0.25f * _trim(2) *cosf(_trim(6)), 0.25f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + matrix = matrix::Matrix(tiltrotor_vtol); + + // Temporarily disable a few controls (WIP) + for (size_t j = 4; j < 8; j++) { + matrix(3, j) = 0.0f; + matrix(4, j) = 0.0f; + matrix(5, j) = 0.0f; + } + + + _updated = false; + return true; +} + +void +ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + + _updated = true; +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp new file mode 100644 index 0000000..3c266b8 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessTiltrotorVTOL.hpp + * + * Actuator effectiveness for tiltrotor VTOL + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessTiltrotorVTOL(); + virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; + + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + void setFlightPhase(const FlightPhase &flight_phase) override; + + int numActuators() const override { return 10; } +protected: + bool _updated{true}; +}; diff --git a/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt new file mode 100644 index 0000000..50eec6e --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# 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(ActuatorEffectiveness + ActuatorEffectiveness.hpp + ActuatorEffectivenessMultirotor.cpp + ActuatorEffectivenessMultirotor.hpp + ActuatorEffectivenessStandardVTOL.cpp + ActuatorEffectivenessStandardVTOL.hpp + ActuatorEffectivenessTiltrotorVTOL.cpp + ActuatorEffectivenessTiltrotorVTOL.hpp +) + +target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(ActuatorEffectiveness + PRIVATE + mathlib + ControlAllocation +) + +# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/prometheus_px4/src/modules/control_allocator/CMakeLists.txt b/src/prometheus_px4/src/modules/control_allocator/CMakeLists.txt new file mode 100644 index 0000000..06a0ff0 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# 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_directories(${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(ActuatorEffectiveness) +add_subdirectory(ControlAllocation) + +px4_add_module( + MODULE modules__control_allocator + MAIN control_allocator + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + ControlAllocator.cpp + ControlAllocator.hpp + DEPENDS + mathlib + ActuatorEffectiveness + ControlAllocation + mixer + px4_work_queue +) diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/CMakeLists.txt new file mode 100644 index 0000000..ecf5f0d --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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(ControlAllocation + ControlAllocation.cpp + ControlAllocation.hpp + ControlAllocationPseudoInverse.cpp + ControlAllocationPseudoInverse.hpp + ControlAllocationSequentialDesaturation.cpp + ControlAllocationSequentialDesaturation.hpp +) +target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(ControlAllocation PRIVATE mathlib) + +px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp new file mode 100644 index 0000000..9fac9f5 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 ControlAllocation.cpp + * + * Interface for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include "ControlAllocation.hpp" + +void +ControlAllocation::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators) +{ + _effectiveness = effectiveness; + _actuator_trim = actuator_trim; + clipActuatorSetpoint(_actuator_trim); + _control_trim = _effectiveness * _actuator_trim; + _num_actuators = num_actuators; + + // make sure unused actuators are initialized to trim + for (int i = num_actuators; i < NUM_ACTUATORS; ++i) { + _actuator_sp(i) = _actuator_trim(i); + } +} + +void +ControlAllocation::setActuatorSetpoint( + const matrix::Vector &actuator_sp) +{ + // Set actuator setpoint + _actuator_sp = actuator_sp; + + // Clip + clipActuatorSetpoint(_actuator_sp); + + // Compute achieved control + _control_allocated = _effectiveness * _actuator_sp; + +} + +void +ControlAllocation::clipActuatorSetpoint(matrix::Vector &actuator) const +{ + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_max(i) < _actuator_min(i)) { + actuator(i) = _actuator_trim(i); + + } else if (actuator(i) < _actuator_min(i)) { + actuator(i) = _actuator_min(i); + + } else if (actuator(i) > _actuator_max(i)) { + actuator(i) = _actuator_max(i); + } + } +} + +matrix::Vector +ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector &actuator) +const +{ + matrix::Vector actuator_normalized; + + for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) { + if (_actuator_min(i) < _actuator_max(i)) { + actuator_normalized(i) = -1.0f + 2.0f * (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + + } else { + actuator_normalized(i) = -1.0f + 2.0f * (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + } + } + + return actuator_normalized; +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp new file mode 100644 index 0000000..86a2cfc --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * 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 ControlAllocation.hpp + * + * Interface for Control Allocation Algorithms + * + * Implementers of this interface are expected to update the members + * of this base class in the `allocate` method. + * + * Example usage: + * ``` + * [...] + * // Initialization + * ControlAllocationMethodImpl alloc(); + * alloc.setEffectivenessMatrix(effectiveness, actuator_trim); + * alloc.setActuatorMin(actuator_min); + * alloc.setActuatorMin(actuator_max); + * + * while (1) { + * [...] + * + * // Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint + * alloc.setControlSetpoint(control_sp); + * alloc.allocate(); + * actuator_sp = alloc.getActuatorSetpoint(); + * + * // Check if the control setpoint was fully allocated + * unallocated_control = control_sp - alloc.getAllocatedControl() + * + * [...] + * } + * ``` + * + * + * @author Julien Lecoeur + */ + +#pragma once + +#include +#include + +class ControlAllocation +{ +public: + ControlAllocation() = default; + virtual ~ControlAllocation() = default; + + static constexpr uint8_t NUM_ACTUATORS = 16; + static constexpr uint8_t NUM_AXES = 6; + + typedef matrix::Vector ActuatorVector; + + enum ControlAxis { + ROLL = 0, + PITCH, + YAW, + THRUST_X, + THRUST_Y, + THRUST_Z + }; + + /** + * Allocate control setpoint to actuators + * + * @param control_setpoint Desired control setpoint vector (input) + */ + virtual void allocate() = 0; + + /** + * Set the control effectiveness matrix + * + * @param B Effectiveness matrix + */ + virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators); + + /** + * Get the allocated actuator vector + * + * @return Actuator vector + */ + const matrix::Vector &getActuatorSetpoint() const { return _actuator_sp; } + + /** + * Set the desired control vector + * + * @param Control vector + */ + void setControlSetpoint(const matrix::Vector &control) { _control_sp = control; } + + /** + * Set the desired control vector + * + * @param Control vector + */ + const matrix::Vector &getControlSetpoint() const { return _control_sp; } + + /** + * Get the allocated control vector + * + * @return Control vector + */ + const matrix::Vector &getAllocatedControl() const { return _control_allocated; } + + /** + * Get the control effectiveness matrix + * + * @return Effectiveness matrix + */ + const matrix::Matrix &getEffectivenessMatrix() const { return _effectiveness; } + + /** + * Set the minimum actuator values + * + * @param actuator_min Minimum actuator values + */ + void setActuatorMin(const matrix::Vector &actuator_min) { _actuator_min = actuator_min; } + + /** + * Get the minimum actuator values + * + * @return Minimum actuator values + */ + const matrix::Vector &getActuatorMin() const { return _actuator_min; } + + /** + * Set the maximum actuator values + * + * @param actuator_max Maximum actuator values + */ + void setActuatorMax(const matrix::Vector &actuator_max) { _actuator_max = actuator_max; } + + /** + * Get the maximum actuator values + * + * @return Maximum actuator values + */ + const matrix::Vector &getActuatorMax() const { return _actuator_max; } + + /** + * Set the current actuator setpoint. + * + * Use this when a new allocation method is started to initialize it properly. + * In most cases, it is not needed to call this method before `allocate()`. + * Indeed the previous actuator setpoint is expected to be stored during calls to `allocate()`. + * + * @param actuator_sp Actuator setpoint + */ + void setActuatorSetpoint(const matrix::Vector &actuator_sp); + + /** + * Clip the actuator setpoint between minimum and maximum values. + * + * The output is in the range [min; max] + * + * @param actuator Actuator vector to clip + */ + void clipActuatorSetpoint(matrix::Vector &actuator) const; + + /** + * Normalize the actuator setpoint between minimum and maximum values. + * + * The output is in the range [-1; +1] + * + * @param actuator Actuator vector to normalize + * + * @return Clipped actuator setpoint + */ + matrix::Vector normalizeActuatorSetpoint(const matrix::Vector &actuator) + const; + + virtual void updateParameters() {} + + int numConfiguredActuators() const { return _num_actuators; } + +protected: + matrix::Matrix _effectiveness; //< Effectiveness matrix + matrix::Vector _actuator_trim; //< Neutral actuator values + matrix::Vector _actuator_min; //< Minimum actuator values + matrix::Vector _actuator_max; //< Maximum actuator values + matrix::Vector _actuator_sp; //< Actuator setpoint + matrix::Vector _control_sp; //< Control setpoint + matrix::Vector _control_allocated; //< Allocated control + matrix::Vector _control_trim; //< Control at trim actuator values + int _num_actuators{0}; +}; diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp new file mode 100644 index 0000000..b613e77 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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 ControlAllocationPseudoInverse.hpp + * + * Simple Control Allocation Algorithm + * + * @author Julien Lecoeur + */ + +#include "ControlAllocationPseudoInverse.hpp" + +void +ControlAllocationPseudoInverse::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators) +{ + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, num_actuators); + _mix_update_needed = true; +} + +void +ControlAllocationPseudoInverse::updatePseudoInverse() +{ + if (_mix_update_needed) { + _mix = matrix::geninv(_effectiveness); + _mix_update_needed = false; + } +} + +void +ControlAllocationPseudoInverse::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + // Allocate + _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); + + // Clip + clipActuatorSetpoint(_actuator_sp); + + // Compute achieved control + _control_allocated = _effectiveness * _actuator_sp; +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp new file mode 100644 index 0000000..148b5af --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * 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 ControlAllocationPseudoInverse.hpp + * + * Simple Control Allocation Algorithm + * + * It computes the pseudo-inverse of the effectiveness matrix + * Actuator saturation is handled by simple clipping, do not + * expect good performance in case of actuator saturation. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ControlAllocation.hpp" + +class ControlAllocationPseudoInverse: public ControlAllocation +{ +public: + ControlAllocationPseudoInverse() = default; + virtual ~ControlAllocationPseudoInverse() = default; + + virtual void allocate() override; + virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators) override; + +protected: + matrix::Matrix _mix; + + bool _mix_update_needed{false}; + + /** + * Recalculate pseudo inverse if required. + * + */ + void updatePseudoInverse(); +}; diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp new file mode 100644 index 0000000..aba3cf8 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 ControlAllocationTest.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include + +using namespace matrix; + +TEST(ControlAllocationTest, AllZeroCase) +{ + ControlAllocationPseudoInverse method; + + matrix::Vector control_sp; + matrix::Vector control_allocated; + matrix::Vector control_allocated_expected; + matrix::Matrix effectiveness; + matrix::Vector actuator_sp; + matrix::Vector actuator_trim; + matrix::Vector actuator_sp_expected; + + method.setEffectivenessMatrix(effectiveness, actuator_trim, 16); + method.setControlSetpoint(control_sp); + method.allocate(); + actuator_sp = method.getActuatorSetpoint(); + control_allocated_expected = method.getAllocatedControl(); + + EXPECT_EQ(actuator_sp, actuator_sp_expected); + EXPECT_EQ(control_allocated, control_allocated_expected); +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp new file mode 100644 index 0000000..a497fd8 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * 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 ControlAllocationSequentialDesaturation.cpp + * + * @author Roman Bapst + * @author Beat Küng + */ + +#include "ControlAllocationSequentialDesaturation.hpp" + + +void +ControlAllocationSequentialDesaturation::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + switch (_param_mc_airmode.get()) { + case 1: + mixAirmodeRP(); + break; + + case 2: + mixAirmodeRPY(); + break; + + default: + mixAirmodeDisabled(); + break; + } + + // TODO: thrust model (THR_MDL_FAC) + + // Clip + clipActuatorSetpoint(_actuator_sp); + + // Compute achieved control + _control_allocated = _effectiveness * _actuator_sp; +} + +void ControlAllocationSequentialDesaturation::desaturateActuators( + ActuatorVector &actuator_sp, + const ActuatorVector &desaturation_vector, bool increase_only) +{ + float gain = computeDesaturationGain(desaturation_vector, actuator_sp); + + if (increase_only && gain < 0.f) { + return; + } + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } + + gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp); + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } +} + +float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector, + const ActuatorVector &actuator_sp) +{ + float k_min = 0.f; + float k_max = 0.f; + + for (int i = 0; i < _num_actuators; i++) { + // Avoid division by zero. If desaturation_vector(i) is zero, there's nothing we can do to unsaturate anyway + if (fabsf(desaturation_vector(i)) < FLT_EPSILON) { + continue; + } + + if (actuator_sp(i) < _actuator_min(i)) { + float k = (_actuator_min(i) - actuator_sp(i)) / desaturation_vector(i); + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + } + + if (actuator_sp(i) > _actuator_max(i)) { + float k = (_actuator_max(i) - actuator_sp(i)) / desaturation_vector(i); + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + } + } + + // Reduce the saturation as much as possible + return k_min + k_max; +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRP() +{ + // Airmode for roll and pitch, but not yaw + + // Mix without yaw + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRPY() +{ + // Airmode for roll, pitch and yaw + + // Do full mixing + ActuatorVector thrust_z; + ActuatorVector yaw; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + yaw(i) = _mix(i, ControlAxis::YAW); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Unsaturate yaw (in case upper and lower bounds are exceeded) + // to prioritize roll/pitch over yaw. + desaturateActuators(_actuator_sp, yaw); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeDisabled() +{ + // Airmode disabled: never allow to increase the thrust to unsaturate a motor + + // Mix without yaw + ActuatorVector thrust_z; + ActuatorVector roll; + ActuatorVector pitch; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + roll(i) = _mix(i, ControlAxis::ROLL); + pitch(i) = _mix(i, ControlAxis::PITCH); + } + + // only reduce thrust + desaturateActuators(_actuator_sp, thrust_z, true); + + // Reduce roll/pitch acceleration if needed to unsaturate + desaturateActuators(_actuator_sp, roll); + desaturateActuators(_actuator_sp, pitch); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixYaw() +{ + // Add yaw to outputs + ActuatorVector yaw; + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + yaw(i) = _mix(i, ControlAxis::YAW); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + // and allow some yaw response at maximum thrust + ActuatorVector max_prev = _actuator_max; + _actuator_max += (_actuator_max - _actuator_min) * 0.15f; + desaturateActuators(_actuator_sp, yaw); + _actuator_max = max_prev; + + // reduce thrust only + desaturateActuators(_actuator_sp, thrust_z, true); +} + +void +ControlAllocationSequentialDesaturation::updateParameters() +{ + updateParams(); +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp new file mode 100644 index 0000000..53c422c --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 ControlAllocationSequentialDesaturation.hpp + * + * Control Allocation Algorithm which sequentially modifies control demands in order to + * eliminate the saturation of the actuator setpoint vector. + * + * + * @author Roman Bapst + */ + +#pragma once + +#include "ControlAllocationPseudoInverse.hpp" + +#include + +class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams +{ +public: + + ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {} + virtual ~ControlAllocationSequentialDesaturation() = default; + + void allocate() override; + + void updateParameters() override; +private: + + /** + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular + * acceleration on a specific axis. + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the + * saturation will be minimized by shifting the vertical thrust setpoint, without changing the + * roll/pitch/yaw accelerations. + * + * Note that as we only slide along the given axis, in extreme cases outputs can still contain values + * outside of [min_output, max_output]. + * + * @param actuator_sp Actuator setpoint, vector that is modified + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale + * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector + */ + void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, + bool increase_only = false); + + /** + * Computes the gain k by which desaturation_vector has to be multiplied + * in order to unsaturate the output that has the greatest saturation. + * + * @return desaturation gain + */ + float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: airmode for roll/pitch: + * thrust is increased/decreased as much as required to meet the demanded roll/pitch. + * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + */ + void mixAirmodeRP(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: full airmode for roll/pitch/yaw: + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. + */ + void mixAirmodeRPY(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + * Thrust can be reduced to unsaturate the upper side. + * @see mixYaw() for the exact yaw behavior. + */ + void mixAirmodeDisabled(); + + /** + * Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust). + * + * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + * some yaw control on the upper end. On the lower end thrust will never be increased, + * but yaw is decreased as much as required. + */ + void mixYaw(); + + DEFINE_PARAMETERS( + (ParamInt) _param_mc_airmode ///< air-mode + ); +}; diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocator.cpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocator.cpp new file mode 100644 index 0000000..1bb0ae3 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocator.cpp @@ -0,0 +1,569 @@ +/**************************************************************************** + * + * 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 ControlAllocator.cpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#include "ControlAllocator.hpp" + +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; + +ControlAllocator::ControlAllocator() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + parameters_updated(); +} + +ControlAllocator::~ControlAllocator() +{ + delete _control_allocation; + delete _actuator_effectiveness; + + perf_free(_loop_perf); +} + +bool +ControlAllocator::init() +{ + if (!_vehicle_torque_setpoint_sub.registerCallback()) { + PX4_ERR("vehicle_torque_setpoint callback registration failed!"); + return false; + } + + if (!_vehicle_thrust_setpoint_sub.registerCallback()) { + PX4_ERR("vehicle_thrust_setpoint callback registration failed!"); + return false; + } + + return true; +} + +void +ControlAllocator::parameters_updated() +{ + // Allocation method & effectiveness source + // Do this first: in case a new method is loaded, it will be configured below + update_effectiveness_source(); + update_allocation_method(); + + if (_control_allocation == nullptr) { + return; + } + + // Minimum actuator values + matrix::Vector actuator_min; + actuator_min(0) = _param_ca_act0_min.get(); + actuator_min(1) = _param_ca_act1_min.get(); + actuator_min(2) = _param_ca_act2_min.get(); + actuator_min(3) = _param_ca_act3_min.get(); + actuator_min(4) = _param_ca_act4_min.get(); + actuator_min(5) = _param_ca_act5_min.get(); + actuator_min(6) = _param_ca_act6_min.get(); + actuator_min(7) = _param_ca_act7_min.get(); + actuator_min(8) = _param_ca_act8_min.get(); + actuator_min(9) = _param_ca_act9_min.get(); + actuator_min(10) = _param_ca_act10_min.get(); + actuator_min(11) = _param_ca_act11_min.get(); + actuator_min(12) = _param_ca_act12_min.get(); + actuator_min(13) = _param_ca_act13_min.get(); + actuator_min(14) = _param_ca_act14_min.get(); + actuator_min(15) = _param_ca_act15_min.get(); + _control_allocation->setActuatorMin(actuator_min); + + // Maximum actuator values + matrix::Vector actuator_max; + actuator_max(0) = _param_ca_act0_max.get(); + actuator_max(1) = _param_ca_act1_max.get(); + actuator_max(2) = _param_ca_act2_max.get(); + actuator_max(3) = _param_ca_act3_max.get(); + actuator_max(4) = _param_ca_act4_max.get(); + actuator_max(5) = _param_ca_act5_max.get(); + actuator_max(6) = _param_ca_act6_max.get(); + actuator_max(7) = _param_ca_act7_max.get(); + actuator_max(8) = _param_ca_act8_max.get(); + actuator_max(9) = _param_ca_act9_max.get(); + actuator_max(10) = _param_ca_act10_max.get(); + actuator_max(11) = _param_ca_act11_max.get(); + actuator_max(12) = _param_ca_act12_max.get(); + actuator_max(13) = _param_ca_act13_max.get(); + actuator_max(14) = _param_ca_act14_max.get(); + actuator_max(15) = _param_ca_act15_max.get(); + _control_allocation->setActuatorMax(actuator_max); +} + +void +ControlAllocator::update_allocation_method() +{ + AllocationMethod method = (AllocationMethod)_param_ca_method.get(); + + if (_allocation_method_id != method) { + + // Save current state + matrix::Vector actuator_sp; + + if (_control_allocation != nullptr) { + actuator_sp = _control_allocation->getActuatorSetpoint(); + } + + // try to instanciate new allocation method + ControlAllocation *tmp = nullptr; + + switch (method) { + case AllocationMethod::PSEUDO_INVERSE: + tmp = new ControlAllocationPseudoInverse(); + break; + + case AllocationMethod::SEQUENTIAL_DESATURATION: + tmp = new ControlAllocationSequentialDesaturation(); + break; + + default: + PX4_ERR("Unknown allocation method"); + break; + } + + // Replace previous method with new one + if (tmp == nullptr) { + // It did not work, forget about it + PX4_ERR("Control allocation init failed"); + _param_ca_method.set((int)_allocation_method_id); + + } else { + // Swap allocation methods + delete _control_allocation; + _control_allocation = tmp; + + // Save method id + _allocation_method_id = method; + + // Configure new allocation method + _control_allocation->setActuatorSetpoint(actuator_sp); + } + } +} + +void +ControlAllocator::update_effectiveness_source() +{ + EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get(); + + if (_effectiveness_source_id != source) { + + // try to instanciate new effectiveness source + ActuatorEffectiveness *tmp = nullptr; + + switch (source) { + case EffectivenessSource::NONE: + case EffectivenessSource::MULTIROTOR: + tmp = new ActuatorEffectivenessMultirotor(); + break; + + case EffectivenessSource::STANDARD_VTOL: + tmp = new ActuatorEffectivenessStandardVTOL(); + break; + + case EffectivenessSource::TILTROTOR_VTOL: + tmp = new ActuatorEffectivenessTiltrotorVTOL(); + break; + + default: + PX4_ERR("Unknown airframe"); + break; + } + + // Replace previous source with new one + if (tmp == nullptr) { + // It did not work, forget about it + PX4_ERR("Actuator effectiveness init failed"); + _param_ca_airframe.set((int)_effectiveness_source_id); + + } else { + // Swap effectiveness sources + delete _actuator_effectiveness; + _actuator_effectiveness = tmp; + + // Save source id + _effectiveness_source_id = source; + } + } +} + +void +ControlAllocator::Run() +{ + if (should_exit()) { + _vehicle_torque_setpoint_sub.unregisterCallback(); + _vehicle_thrust_setpoint_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(¶m_update); + + if (_control_allocation) { + _control_allocation->updateParameters(); + } + + updateParams(); + parameters_updated(); + } + + if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) { + return; + } + + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + + ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; + + // Check if the current flight phase is HOVER or FIXED_WING + if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT; + + } else { + flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT; + } + + // Special cases for VTOL in transition + if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { + if (vehicle_status.in_transition_to_fw) { + flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF; + + } else { + flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF; + } + } + + // Forward to effectiveness source + _actuator_effectiveness->setFlightPhase(flight_phase); + } + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + + bool do_update = false; + vehicle_torque_setpoint_s vehicle_torque_setpoint; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + + // Run allocator on torque changes + if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) { + _torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz); + + do_update = true; + _timestamp_sample = vehicle_torque_setpoint.timestamp_sample; + + } + + // Also run allocator on thrust setpoint changes if the torque setpoint + // has not been updated for more than 5ms + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) { + _thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz); + + if (dt > 5_ms) { + do_update = true; + _timestamp_sample = vehicle_thrust_setpoint.timestamp_sample; + } + } + + if (do_update) { + _last_run = now; + + update_effectiveness_matrix_if_needed(); + + // Set control setpoint vector + matrix::Vector c; + c(0) = _torque_sp(0); + c(1) = _torque_sp(1); + c(2) = _torque_sp(2); + c(3) = _thrust_sp(0); + c(4) = _thrust_sp(1); + c(5) = _thrust_sp(2); + _control_allocation->setControlSetpoint(c); + + // Do allocation + _control_allocation->allocate(); + + // Publish actuator setpoint and allocator status + publish_actuator_setpoint(); + publish_control_allocator_status(); + + // Publish on legacy topics for compatibility with + // the current mixer system and multicopter controller + // TODO: remove + publish_legacy_actuator_controls(); + } + + perf_end(_loop_perf); +} + +void +ControlAllocator::update_effectiveness_matrix_if_needed() +{ + matrix::Matrix effectiveness; + + if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) { + const matrix::Vector &trim = _actuator_effectiveness->getActuatorTrim(); + + // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) + matrix::Vector actuator_max = _control_allocation->getActuatorMax(); + matrix::Vector actuator_min = _control_allocation->getActuatorMin(); + + for (size_t j = 0; j < NUM_ACTUATORS; j++) { + if (actuator_min(j) >= actuator_max(j)) { + for (size_t i = 0; i < NUM_AXES; i++) { + effectiveness(i, j) = 0.0f; + } + } + } + + // Assign control effectiveness matrix + _control_allocation->setEffectivenessMatrix(effectiveness, trim, _actuator_effectiveness->numActuators()); + } +} + +void +ControlAllocator::publish_actuator_setpoint() +{ + matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); + + vehicle_actuator_setpoint_s vehicle_actuator_setpoint{}; + vehicle_actuator_setpoint.timestamp = hrt_absolute_time(); + vehicle_actuator_setpoint.timestamp_sample = _timestamp_sample; + actuator_sp.copyTo(vehicle_actuator_setpoint.actuator); + + _vehicle_actuator_setpoint_pub.publish(vehicle_actuator_setpoint); +} + +void +ControlAllocator::publish_control_allocator_status() +{ + control_allocator_status_s control_allocator_status{}; + control_allocator_status.timestamp = hrt_absolute_time(); + + // Allocated control + const matrix::Vector &allocated_control = _control_allocation->getAllocatedControl(); + control_allocator_status.allocated_torque[0] = allocated_control(0); + control_allocator_status.allocated_torque[1] = allocated_control(1); + control_allocator_status.allocated_torque[2] = allocated_control(2); + control_allocator_status.allocated_thrust[0] = allocated_control(3); + control_allocator_status.allocated_thrust[1] = allocated_control(4); + control_allocator_status.allocated_thrust[2] = allocated_control(5); + + // Unallocated control + matrix::Vector unallocated_control = _control_allocation->getControlSetpoint() - allocated_control; + control_allocator_status.unallocated_torque[0] = unallocated_control(0); + control_allocator_status.unallocated_torque[1] = unallocated_control(1); + control_allocator_status.unallocated_torque[2] = unallocated_control(2); + control_allocator_status.unallocated_thrust[0] = unallocated_control(3); + control_allocator_status.unallocated_thrust[1] = unallocated_control(4); + control_allocator_status.unallocated_thrust[2] = unallocated_control(5); + + // Allocation success flags + control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1), + unallocated_control(2)).norm_squared() < 1e-6f); + control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), + unallocated_control(5)).norm_squared() < 1e-6f); + + // Actuator saturation + const matrix::Vector &actuator_sp = _control_allocation->getActuatorSetpoint(); + const matrix::Vector &actuator_min = _control_allocation->getActuatorMin(); + const matrix::Vector &actuator_max = _control_allocation->getActuatorMax(); + + for (size_t i = 0; i < NUM_ACTUATORS; i++) { + if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) { + control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_UPPER; + + } else if (actuator_sp(i) < (actuator_min(i) + FLT_EPSILON)) { + control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_LOWER; + } + } + + _control_allocator_status_pub.publish(control_allocator_status); +} + +void +ControlAllocator::publish_legacy_actuator_controls() +{ + // For compatibility with the current mixer system, + // publish normalized version on actuator_controls_4/5 + actuator_controls_s actuator_controls_4{}; + actuator_controls_s actuator_controls_5{}; + actuator_controls_4.timestamp = hrt_absolute_time(); + actuator_controls_5.timestamp = hrt_absolute_time(); + actuator_controls_4.timestamp_sample = _timestamp_sample; + actuator_controls_5.timestamp_sample = _timestamp_sample; + + matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); + matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint( + actuator_sp); + + for (size_t i = 0; i < 8; i++) { + actuator_controls_4.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i))) ? actuator_sp_normalized(i) : 0.0f; + actuator_controls_5.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i + 8))) ? actuator_sp_normalized(i + 8) : 0.0f; + } + + _actuator_controls_4_pub.publish(actuator_controls_4); + _actuator_controls_5_pub.publish(actuator_controls_5); +} + +int ControlAllocator::task_spawn(int argc, char *argv[]) +{ + ControlAllocator *instance = new ControlAllocator(); + + 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 ControlAllocator::print_status() +{ + PX4_INFO("Running"); + + // Print current allocation method + switch (_allocation_method_id) { + case AllocationMethod::NONE: + PX4_INFO("Method: None"); + break; + + case AllocationMethod::PSEUDO_INVERSE: + PX4_INFO("Method: Pseudo-inverse"); + break; + + case AllocationMethod::SEQUENTIAL_DESATURATION: + PX4_INFO("Method: Sequential desaturation"); + break; + } + + // Print current airframe + switch ((EffectivenessSource)_param_ca_airframe.get()) { + case EffectivenessSource::NONE: + PX4_INFO("EffectivenessSource: None"); + break; + + case EffectivenessSource::MULTIROTOR: + PX4_INFO("EffectivenessSource: MC parameters"); + break; + + case EffectivenessSource::STANDARD_VTOL: + PX4_INFO("EffectivenessSource: Standard VTOL"); + break; + + case EffectivenessSource::TILTROTOR_VTOL: + PX4_INFO("EffectivenessSource: Tiltrotor VTOL"); + break; + } + + // Print current effectiveness matrix + if (_control_allocation != nullptr) { + const matrix::Matrix &effectiveness = _control_allocation->getEffectivenessMatrix(); + PX4_INFO("Effectiveness.T ="); + effectiveness.T().print(); + PX4_INFO("Configured actuators: %i", _control_allocation->numConfiguredActuators()); + } + + // Print perf + perf_print_counter(_loop_perf); + + return 0; +} + +int ControlAllocator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int ControlAllocator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements control allocation. It takes torque and thrust setpoints +as inputs and outputs actuator setpoint messages. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Control Allocator app start / stop handling function + */ +extern "C" __EXPORT int control_allocator_main(int argc, char *argv[]); + +int control_allocator_main(int argc, char *argv[]) +{ + return ControlAllocator::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/control_allocator/ControlAllocator.hpp b/src/prometheus_px4/src/modules/control_allocator/ControlAllocator.hpp new file mode 100644 index 0000000..f46570f --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/ControlAllocator.hpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * 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 ControlAllocator.hpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ControlAllocator : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + ControlAllocator(); + + virtual ~ControlAllocator(); + + /** @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(); + + static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; + static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES; + +private: + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void update_allocation_method(); + void update_effectiveness_source(); + + void update_effectiveness_matrix_if_needed(); + + void publish_actuator_setpoint(); + void publish_control_allocator_status(); + + void publish_legacy_actuator_controls(); + void publish_legacy_multirotor_motor_limits(); + + enum class AllocationMethod { + NONE = -1, + PSEUDO_INVERSE = 0, + SEQUENTIAL_DESATURATION = 1, + }; + + AllocationMethod _allocation_method_id{AllocationMethod::NONE}; + ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations + + enum class EffectivenessSource { + NONE = -1, + MULTIROTOR = 0, + STANDARD_VTOL = 1, + TILTROTOR_VTOL = 2, + }; + + EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; + ActuatorEffectiveness *_actuator_effectiveness{nullptr}; ///< class providing actuator effectiveness + + // Inputs + uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */ + uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */ + + // Outputs + uORB::Publication _vehicle_actuator_setpoint_pub{ORB_ID(vehicle_actuator_setpoint)}; /**< actuator setpoint publication */ + uORB::Publication _control_allocator_status_pub{ORB_ID(control_allocator_status)}; /**< actuator setpoint publication */ + + // actuator_controls publication handles (temporary hack to plug actuator_setpoint into the mixer system) + uORB::Publication _actuator_controls_4_pub{ORB_ID(actuator_controls_4)}; /**< actuator controls 4 publication */ + uORB::Publication _actuator_controls_5_pub{ORB_ID(actuator_controls_5)}; /**< actuator controls 5 publication */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + matrix::Vector3f _torque_sp; + matrix::Vector3f _thrust_sp; + + // float _battery_scale_factor{1.0f}; + // float _airspeed_scale_factor{1.0f}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + hrt_abstime _last_run{0}; + hrt_abstime _timestamp_sample{0}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_airframe, + (ParamInt) _param_ca_method, + (ParamBool) _param_ca_bat_scale_en, + (ParamBool) _param_ca_air_scale_en, + (ParamFloat) _param_ca_act0_min, + (ParamFloat) _param_ca_act1_min, + (ParamFloat) _param_ca_act2_min, + (ParamFloat) _param_ca_act3_min, + (ParamFloat) _param_ca_act4_min, + (ParamFloat) _param_ca_act5_min, + (ParamFloat) _param_ca_act6_min, + (ParamFloat) _param_ca_act7_min, + (ParamFloat) _param_ca_act8_min, + (ParamFloat) _param_ca_act9_min, + (ParamFloat) _param_ca_act10_min, + (ParamFloat) _param_ca_act11_min, + (ParamFloat) _param_ca_act12_min, + (ParamFloat) _param_ca_act13_min, + (ParamFloat) _param_ca_act14_min, + (ParamFloat) _param_ca_act15_min, + (ParamFloat) _param_ca_act0_max, + (ParamFloat) _param_ca_act1_max, + (ParamFloat) _param_ca_act2_max, + (ParamFloat) _param_ca_act3_max, + (ParamFloat) _param_ca_act4_max, + (ParamFloat) _param_ca_act5_max, + (ParamFloat) _param_ca_act6_max, + (ParamFloat) _param_ca_act7_max, + (ParamFloat) _param_ca_act8_max, + (ParamFloat) _param_ca_act9_max, + (ParamFloat) _param_ca_act10_max, + (ParamFloat) _param_ca_act11_max, + (ParamFloat) _param_ca_act12_max, + (ParamFloat) _param_ca_act13_max, + (ParamFloat) _param_ca_act14_max, + (ParamFloat) _param_ca_act15_max + ) + +}; diff --git a/src/prometheus_px4/src/modules/control_allocator/control_allocator_params.c b/src/prometheus_px4/src/modules/control_allocator/control_allocator_params.c new file mode 100644 index 0000000..c1384d4 --- /dev/null +++ b/src/prometheus_px4/src/modules/control_allocator/control_allocator_params.c @@ -0,0 +1,314 @@ +/**************************************************************************** + * + * 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 control_allocator_params.c + * + * Parameters for the control allocator. + * + * @author Julien Lecoeur + */ + + +/** + * Airframe ID + * + * This is used to retrieve pre-computed control effectiveness matrix + * + * @min 0 + * @max 2 + * @value 0 Multirotor + * @value 1 Standard VTOL (WIP) + * @value 2 Tiltrotor VTOL (WIP) + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_AIRFRAME, 0); + +/** + * Control allocation method + * + * @value 0 Pseudo-inverse with output clipping (default) + * @value 1 Pseudo-inverse with sequential desaturation technique + * @min 0 + * @max 1 + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_METHOD, 0); + +/** + * Battery power level scaler + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The copter + * should constantly behave as if it was fully charged with reduced max acceleration + * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_BAT_SCALE_EN, 0); + +/** + * Airspeed scaler + * + * This compensates for the variation of flap effectiveness with airspeed. + * + * @boolean + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_AIR_SCALE_EN, 0); + +/** + * Minimum value for actuator 0 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT0_MIN, 0.0); + +/** + * Minimum value for actuator 1 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT1_MIN, 0.0); + +/** + * Minimum value for actuator 2 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT2_MIN, 0.0); + +/** + * Minimum value for actuator 3 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT3_MIN, 0.0); + +/** + * Minimum value for actuator 4 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT4_MIN, 0.0); + +/** + * Minimum value for actuator 5 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT5_MIN, 0.0); + +/** + * Minimum value for actuator 6 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT6_MIN, 0.0); + +/** + * Minimum value for actuator 7 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT7_MIN, 0.0); + +/** + * Minimum value for actuator 8 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT8_MIN, 0.0); + +/** + * Minimum value for actuator 9 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT9_MIN, 0.0); + +/** + * Minimum value for actuator 10 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT10_MIN, 0.0); + +/** + * Minimum value for actuator 11 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT11_MIN, 0.0); + +/** + * Minimum value for actuator 12 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT12_MIN, 0.0); + +/** + * Minimum value for actuator 13 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT13_MIN, 0.0); + +/** + * Minimum value for actuator 14 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT14_MIN, 0.0); + +/** + * Minimum value for actuator 15 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT15_MIN, 0.0); + +/** + * Maximum value for actuator 0 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT0_MAX, 0.0); + +/** + * Maximum value for actuator 1 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT1_MAX, 0.0); + +/** + * Maximum value for actuator 2 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT2_MAX, 0.0); + +/** + * Maximum value for actuator 3 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT3_MAX, 0.0); + +/** + * Maximum value for actuator 4 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT4_MAX, 0.0); + +/** + * Maximum value for actuator 5 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT5_MAX, 0.0); + +/** + * Maximum value for actuator 6 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT6_MAX, 0.0); + +/** + * Maximum value for actuator 7 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT7_MAX, 0.0); + +/** + * Maximum value for actuator 8 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT8_MAX, 0.0); + +/** + * Maximum value for actuator 9 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT9_MAX, 0.0); + +/** + * Maximum value for actuator 10 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT10_MAX, 0.0); + +/** + * Maximum value for actuator 11 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT11_MAX, 0.0); + +/** + * Maximum value for actuator 12 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT12_MAX, 0.0); + +/** + * Maximum value for actuator 13 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT13_MAX, 0.0); + +/** + * Maximum value for actuator 14 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT14_MAX, 0.0); + +/** + * Maximum value for actuator 15 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT15_MAX, 0.0); diff --git a/src/prometheus_px4/src/modules/dataman/CMakeLists.txt b/src/prometheus_px4/src/modules/dataman/CMakeLists.txt new file mode 100644 index 0000000..6c0a1ab --- /dev/null +++ b/src/prometheus_px4/src/modules/dataman/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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__dataman + MAIN dataman + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + dataman.cpp + ) diff --git a/src/prometheus_px4/src/modules/dataman/dataman.cpp b/src/prometheus_px4/src/modules/dataman/dataman.cpp new file mode 100644 index 0000000..e07ed10 --- /dev/null +++ b/src/prometheus_px4/src/modules/dataman/dataman.cpp @@ -0,0 +1,1406 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2016, 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 dataman.cpp + * DATAMANAGER driver. + * + * @author Jean Cyr + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler + * @author David Sidrane + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dataman.h" + +__BEGIN_DECLS +__EXPORT int dataman_main(int argc, char *argv[]); +__END_DECLS + +static constexpr int TASK_STACK_SIZE = 1220; + +/* Private File based Operations */ +static ssize_t _file_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, + size_t count); +static ssize_t _file_read(dm_item_t item, unsigned index, void *buf, size_t count); +static int _file_clear(dm_item_t item); +static int _file_restart(dm_reset_reason reason); +static int _file_initialize(unsigned max_offset); +static void _file_shutdown(); + +/* Private Ram based Operations */ +static ssize_t _ram_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, + size_t count); +static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count); +static int _ram_clear(dm_item_t item); +static int _ram_restart(dm_reset_reason reason); +static int _ram_initialize(unsigned max_offset); +static void _ram_shutdown(); + +typedef struct dm_operations_t { + ssize_t (*write)(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count); + ssize_t (*read)(dm_item_t item, unsigned index, void *buf, size_t count); + int (*clear)(dm_item_t item); + int (*restart)(dm_reset_reason reason); + int (*initialize)(unsigned max_offset); + void (*shutdown)(); + int (*wait)(px4_sem_t *sem); +} dm_operations_t; + +static constexpr dm_operations_t dm_file_operations = { + .write = _file_write, + .read = _file_read, + .clear = _file_clear, + .restart = _file_restart, + .initialize = _file_initialize, + .shutdown = _file_shutdown, + .wait = px4_sem_wait, +}; + +static constexpr dm_operations_t dm_ram_operations = { + .write = _ram_write, + .read = _ram_read, + .clear = _ram_clear, + .restart = _ram_restart, + .initialize = _ram_initialize, + .shutdown = _ram_shutdown, + .wait = px4_sem_wait, +}; + +static const dm_operations_t *g_dm_ops; + +static struct { + union { + struct { + int fd; + } file; + struct { + uint8_t *data; + uint8_t *data_end; + } ram; + }; + bool running; +} dm_operations_data; + +/** Types of function calls supported by the worker task */ +typedef enum { + dm_write_func = 0, + dm_read_func, + dm_clear_func, + dm_restart_func, + dm_number_of_funcs +} dm_function_t; + +/** Work task work item */ +typedef struct { + sq_entry_t link; /**< list linkage */ + px4_sem_t wait_sem; + unsigned char first; + unsigned char func; + ssize_t result; + union { + struct { + dm_item_t item; + unsigned index; + dm_persitence_t persistence; + const void *buf; + size_t count; + } write_params; + struct { + dm_item_t item; + unsigned index; + void *buf; + size_t count; + } read_params; + struct { + dm_item_t item; + } clear_params; + struct { + dm_reset_reason reason; + } restart_params; + }; +} work_q_item_t; + +const size_t k_work_item_allocation_chunk_size = 8; + +/* Usage statistics */ +static unsigned g_func_counts[dm_number_of_funcs]; + +/* table of maximum number of instances for each item type */ +static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { + DM_KEY_SAFE_POINTS_MAX, + DM_KEY_FENCE_POINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, + DM_KEY_WAYPOINTS_ONBOARD_MAX, + DM_KEY_MISSION_STATE_MAX, + DM_KEY_COMPAT_MAX +}; + +#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ + +/* Table of the len of each item type */ +static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { + sizeof(struct mission_safe_point_s) + DM_SECTOR_HDR_SIZE, + sizeof(struct mission_fence_point_s) + DM_SECTOR_HDR_SIZE, + sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, + sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, + sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, + sizeof(struct mission_s) + DM_SECTOR_HDR_SIZE, + sizeof(struct dataman_compat_s) + DM_SECTOR_HDR_SIZE +}; + +/* Table of offset for index 0 of each item type */ +static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; + +/* Item type lock mutexes */ +static px4_sem_t *g_item_locks[DM_KEY_NUM_KEYS]; +static px4_sem_t g_sys_state_mutex_mission; +static px4_sem_t g_sys_state_mutex_fence; + +static perf_counter_t _dm_read_perf{nullptr}; +static perf_counter_t _dm_write_perf{nullptr}; + +/* The data manager store file handle and file name */ +static const char *default_device_path = PX4_STORAGEDIR "/dataman"; +static char *k_data_manager_device_path = nullptr; + +static enum { + BACKEND_NONE = 0, + BACKEND_FILE, + BACKEND_RAM, + BACKEND_LAST +} backend = BACKEND_NONE; + +/* The data manager work queues */ + +typedef struct { + sq_queue_t q; /* Nuttx queue */ + px4_sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ + unsigned size; /* Current size of queue */ + unsigned max_size; /* Maximum queue size reached */ +} work_q_t; + +static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ +static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ + +static px4_sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ +static px4_sem_t g_init_sema; + +static bool g_task_should_exit; /**< if true, dataman task should exit */ + +static void init_q(work_q_t *q) +{ + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + px4_sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ + q->size = q->max_size = 0; /* Queue is initially empty */ +} + +static inline void +destroy_q(work_q_t *q) +{ + px4_sem_destroy(&(q->mutex)); /* Destroy the queue lock */ +} + +static inline void +lock_queue(work_q_t *q) +{ + px4_sem_wait(&(q->mutex)); /* Acquire the queue lock */ +} + +static inline void +unlock_queue(work_q_t *q) +{ + px4_sem_post(&(q->mutex)); /* Release the queue lock */ +} + +static work_q_item_t * +create_work_item() +{ + work_q_item_t *item; + + /* Try to reuse item from free item queue */ + lock_queue(&g_free_q); + + if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) { + g_free_q.size--; + } + + unlock_queue(&g_free_q); + + /* If we there weren't any free items then obtain memory for a new ones */ + if (item == nullptr) { + item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); + + if (item) { + item->first = 1; + lock_queue(&g_free_q); + + for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { + (item + i)->first = 0; + sq_addfirst(&(item + i)->link, &(g_free_q.q)); + } + + /* Update the queue size and potentially the maximum queue size */ + g_free_q.size += k_work_item_allocation_chunk_size - 1; + + if (g_free_q.size > g_free_q.max_size) { + g_free_q.max_size = g_free_q.size; + } + + unlock_queue(&g_free_q); + } + } + + /* If we got one then lock the item*/ + if (item) { + px4_sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + + /* item->wait_sem use case is a signal */ + + px4_sem_setprotocol(&item->wait_sem, SEM_PRIO_NONE); + } + + /* return the item pointer, or nullptr if all failed */ + return item; +} + +/* Work queue management functions */ + +static inline void +destroy_work_item(work_q_item_t *item) +{ + px4_sem_destroy(&item->wait_sem); /* Destroy the item lock */ + /* Return the item to the free item queue for later reuse */ + lock_queue(&g_free_q); + sq_addfirst(&item->link, &(g_free_q.q)); + + /* Update the queue size and potentially the maximum queue size */ + if (++g_free_q.size > g_free_q.max_size) { + g_free_q.max_size = g_free_q.size; + } + + unlock_queue(&g_free_q); +} + +static inline work_q_item_t * +dequeue_work_item() +{ + work_q_item_t *work; + + /* retrieve the 1st item on the work queue */ + lock_queue(&g_work_q); + + if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) { + g_work_q.size--; + } + + unlock_queue(&g_work_q); + return work; +} + +static int +enqueue_work_item_and_wait_for_result(work_q_item_t *item) +{ + /* put the work item at the end of the work queue */ + lock_queue(&g_work_q); + sq_addlast(&item->link, &(g_work_q.q)); + + /* Adjust the queue size and potentially the maximum queue size */ + if (++g_work_q.size > g_work_q.max_size) { + g_work_q.max_size = g_work_q.size; + } + + unlock_queue(&g_work_q); + + /* tell the work thread that work is available */ + px4_sem_post(&g_work_queued_sema); + + /* wait for the result */ + px4_sem_wait(&item->wait_sem); + + int result = item->result; + + destroy_work_item(item); + + return result; +} + +static bool is_running() +{ + return dm_operations_data.running; +} + +/* Calculate the offset in file of specific item */ +static int +calculate_offset(dm_item_t item, unsigned index) +{ + + /* Make sure the item type is valid */ + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + + /* Make sure the index for this item type is valid */ + if (index >= g_per_item_max_index[item]) { + return -1; + } + + /* Calculate and return the item index based on type and index */ + return g_key_offsets[item] + (index * g_per_item_size[item]); +} + +/* Each data item is stored as follows + * + * byte 0: Length of user data item + * byte 1: Persistence of this data item + * byte 2: Unused (for future use) + * byte 3: Unused (for future use) + * byte DM_SECTOR_HDR_SIZE... : data item value + * + * The total size must not exceed g_per_item_max_index[item] + */ + +/* write to the data manager RAM buffer */ +static ssize_t _ram_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, + size_t count) +{ + + /* Get the offset for this item */ + int offset = calculate_offset(item, index); + + /* If item type or index out of range, return error */ + if (offset < 0) { + return -1; + } + + /* Make sure caller has not given us more data than we can handle */ + if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + return -E2BIG; + } + + uint8_t *buffer = &dm_operations_data.ram.data[offset]; + + if (buffer > dm_operations_data.ram.data_end) { + return -1; + } + + /* Write out the data, prefixed with length and persistence level */ + buffer[0] = count; + buffer[1] = persistence; + buffer[2] = 0; + buffer[3] = 0; + + if (count > 0) { + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + } + + /* All is well... return the number of user data written */ + return count; +} + +/* write to the data manager file */ +static ssize_t +_file_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count) +{ + unsigned char buffer[g_per_item_size[item]]; + + /* Get the offset for this item */ + const int offset = calculate_offset(item, index); + + /* If item type or index out of range, return error */ + if (offset < 0) { + return -1; + } + + /* Make sure caller has not given us more data than we can handle */ + if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + return -E2BIG; + } + + /* Write out the data, prefixed with length and persistence level */ + buffer[0] = count; + buffer[1] = persistence; + buffer[2] = 0; + buffer[3] = 0; + + if (count > 0) { + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + } + + count += DM_SECTOR_HDR_SIZE; + + if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) != offset) { + return -1; + } + + if ((write(dm_operations_data.file.fd, buffer, count)) != (ssize_t)count) { + return -1; + } + + /* Make sure data is written to physical media */ + fsync(dm_operations_data.file.fd); + + /* All is well... return the number of user data written */ + return count - DM_SECTOR_HDR_SIZE; +} + +/* Retrieve from the data manager RAM buffer*/ +static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count) +{ + /* Get the offset for this item */ + int offset = calculate_offset(item, index); + + /* If item type or index out of range, return error */ + if (offset < 0) { + return -1; + } + + /* Make sure the caller hasn't asked for more data than we can handle */ + if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + return -E2BIG; + } + + /* Read the prefix and data */ + + uint8_t *buffer = &dm_operations_data.ram.data[offset]; + + if (buffer > dm_operations_data.ram.data_end) { + return -1; + } + + /* See if we got data */ + if (buffer[0] > 0) { + /* We got more than requested!!! */ + if (buffer[0] > count) { + return -1; + } + + /* Looks good, copy it to the caller's buffer */ + memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]); + } + + /* Return the number of bytes of caller data read */ + return buffer[0]; +} + +/* Retrieve from the data manager file */ +static ssize_t +_file_read(dm_item_t item, unsigned index, void *buf, size_t count) +{ + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + + unsigned char buffer[g_per_item_size[item]]; + + /* Get the offset for this item */ + int offset = calculate_offset(item, index); + + /* If item type or index out of range, return error */ + if (offset < 0) { + return -1; + } + + /* Make sure the caller hasn't asked for more data than we can handle */ + if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + return -E2BIG; + } + + /* Read the prefix and data */ + int len = -1; + + if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) == offset) { + len = read(dm_operations_data.file.fd, buffer, count + DM_SECTOR_HDR_SIZE); + } + + /* Check for read error */ + if (len < 0) { + return -errno; + } + + /* A zero length entry is a empty entry */ + if (len == 0) { + buffer[0] = 0; + } + + /* See if we got data */ + if (buffer[0] > 0) { + /* We got more than requested!!! */ + if (buffer[0] > count) { + return -1; + } + + /* Looks good, copy it to the caller's buffer */ + memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]); + } + + /* Return the number of bytes of caller data read */ + return buffer[0]; +} + +static int _ram_clear(dm_item_t item) +{ + int i; + int result = 0; + + /* Get the offset of 1st item of this type */ + int offset = calculate_offset(item, 0); + + /* Check for item type out of range */ + if (offset < 0) { + return -1; + } + + /* Clear all items of this type */ + for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { + uint8_t *buf = &dm_operations_data.ram.data[offset]; + + if (buf > dm_operations_data.ram.data_end) { + result = -1; + break; + } + + buf[0] = 0; + offset += g_per_item_size[item]; + } + + return result; +} + +static int +_file_clear(dm_item_t item) +{ + int i, result = 0; + + /* Get the offset of 1st item of this type */ + int offset = calculate_offset(item, 0); + + /* Check for item type out of range */ + if (offset < 0) { + return -1; + } + + /* Clear all items of this type */ + for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { + char buf[1]; + + if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + /* Avoid SD flash wear by only doing writes where necessary */ + if (read(dm_operations_data.file.fd, buf, 1) < 1) { + break; + } + + /* If item has length greater than 0 it needs to be overwritten */ + if (buf[0]) { + if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + buf[0] = 0; + + if (write(dm_operations_data.file.fd, buf, 1) != 1) { + result = -1; + break; + } + } + + offset += g_per_item_size[item]; + } + + /* Make sure data is actually written to physical media */ + fsync(dm_operations_data.file.fd); + return result; +} + +/* Tell the data manager about the type of the last reset */ +static int _ram_restart(dm_reset_reason reason) +{ + uint8_t *buffer = dm_operations_data.ram.data; + + /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ + + /* Loop through all of the data segments and delete those that are not persistent */ + + for (int item = (int)DM_KEY_SAFE_POINTS; item < (int)DM_KEY_NUM_KEYS; item++) { + for (unsigned i = 0; i < g_per_item_max_index[item]; i++) { + /* check if segment contains data */ + if (buffer[0]) { + bool clear_entry = false; + + /* Whether data gets deleted depends on reset type and data segment's persistence setting */ + if (reason == DM_INIT_REASON_POWER_ON) { + if (buffer[1] > DM_PERSIST_POWER_ON_RESET) { + clear_entry = true; + } + + } else { + if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) { + clear_entry = true; + } + } + + /* Set segment to unused if data does not persist */ + if (clear_entry) { + buffer[0] = 0; + } + } + + buffer += g_per_item_size[item]; + } + } + + return 0; +} + +static int +_file_restart(dm_reset_reason reason) +{ + int offset = 0; + int result = 0; + /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ + + /* Loop through all of the data segments and delete those that are not persistent */ + for (int item = (int)DM_KEY_SAFE_POINTS; item < (int)DM_KEY_NUM_KEYS; item++) { + for (unsigned i = 0; i < g_per_item_max_index[item]; i++) { + /* Get data segment at current offset */ + if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) != offset) { + result = -1; + item = DM_KEY_NUM_KEYS; + break; + } + + uint8_t buffer[2]; + ssize_t len = read(dm_operations_data.file.fd, buffer, sizeof(buffer)); + + if (len != sizeof(buffer)) { + result = -1; + item = DM_KEY_NUM_KEYS; + break; + } + + /* check if segment contains data */ + if (buffer[0]) { + bool clear_entry = false; + + /* Whether data gets deleted depends on reset type and data segment's persistence setting */ + if (reason == DM_INIT_REASON_POWER_ON) { + if (buffer[1] > DM_PERSIST_POWER_ON_RESET) { + clear_entry = true; + } + + } else { + if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) { + clear_entry = true; + } + } + + /* Set segment to unused if data does not persist */ + if (clear_entry) { + if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) != offset) { + result = -1; + item = DM_KEY_NUM_KEYS; + break; + } + + buffer[0] = 0; + + len = write(dm_operations_data.file.fd, buffer, 1); + + if (len != 1) { + result = -1; + item = DM_KEY_NUM_KEYS; + break; + } + } + } + + offset += g_per_item_size[item]; + } + } + + fsync(dm_operations_data.file.fd); + + /* tell the caller how it went */ + return result; +} + +static int +_file_initialize(unsigned max_offset) +{ + /* See if the data manage file exists and is a multiple of the sector size */ + dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); + + if (dm_operations_data.file.fd >= 0) { + // Read the mission state and check the hash + struct dataman_compat_s compat_state; + int ret = g_dm_ops->read(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); + + bool incompat = true; + + if (ret == sizeof(compat_state)) { + if (compat_state.key == DM_COMPAT_KEY) { + incompat = false; + } + } + + close(dm_operations_data.file.fd); + + if (incompat) { + unlink(k_data_manager_device_path); + } + } + + /* Open or create the data manager file */ + dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666); + + if (dm_operations_data.file.fd < 0) { + PX4_WARN("Could not open data manager file %s", k_data_manager_device_path); + px4_sem_post(&g_init_sema); /* Don't want to hang startup */ + return -1; + } + + if ((unsigned)lseek(dm_operations_data.file.fd, max_offset, SEEK_SET) != max_offset) { + close(dm_operations_data.file.fd); + PX4_WARN("Could not seek data manager file %s", k_data_manager_device_path); + px4_sem_post(&g_init_sema); /* Don't want to hang startup */ + return -1; + } + + /* Write current compat info */ + struct dataman_compat_s compat_state; + compat_state.key = DM_COMPAT_KEY; + int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, DM_PERSIST_POWER_ON_RESET, &compat_state, sizeof(compat_state)); + + if (ret != sizeof(compat_state)) { + PX4_ERR("Failed writing compat: %d", ret); + } + + fsync(dm_operations_data.file.fd); + dm_operations_data.running = true; + + return 0; +} + +static int +_ram_initialize(unsigned max_offset) +{ + /* In memory */ + dm_operations_data.ram.data = (uint8_t *)malloc(max_offset); + + if (dm_operations_data.ram.data == nullptr) { + PX4_WARN("Could not allocate %u bytes of memory", max_offset); + px4_sem_post(&g_init_sema); /* Don't want to hang startup */ + return -1; + } + + memset(dm_operations_data.ram.data, 0, max_offset); + dm_operations_data.ram.data_end = &dm_operations_data.ram.data[max_offset - 1]; + dm_operations_data.running = true; + + return 0; +} + +static void +_file_shutdown() +{ + close(dm_operations_data.file.fd); + dm_operations_data.running = false; +} + +static void +_ram_shutdown() +{ + free(dm_operations_data.ram.data); + dm_operations_data.running = false; +} + +/** Write to the data manager file */ +__EXPORT ssize_t +dm_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count) +{ + work_q_item_t *work; + + /* Make sure data manager has been started and is not shutting down */ + if (!is_running() || g_task_should_exit) { + return -1; + } + + perf_begin(_dm_write_perf); + + /* get a work item and queue up a write request */ + if ((work = create_work_item()) == nullptr) { + perf_end(_dm_write_perf); + return -1; + } + + work->func = dm_write_func; + work->write_params.item = item; + work->write_params.index = index; + work->write_params.persistence = persistence; + work->write_params.buf = buf; + work->write_params.count = count; + + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + ssize_t ret = (ssize_t)enqueue_work_item_and_wait_for_result(work); + perf_end(_dm_write_perf); + return ret; +} + +/** Retrieve from the data manager file */ +__EXPORT ssize_t +dm_read(dm_item_t item, unsigned index, void *buf, size_t count) +{ + work_q_item_t *work; + + /* Make sure data manager has been started and is not shutting down */ + if (!is_running() || g_task_should_exit) { + return -1; + } + + perf_begin(_dm_read_perf); + + /* get a work item and queue up a read request */ + if ((work = create_work_item()) == nullptr) { + perf_end(_dm_read_perf); + return -1; + } + + work->func = dm_read_func; + work->read_params.item = item; + work->read_params.index = index; + work->read_params.buf = buf; + work->read_params.count = count; + + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + ssize_t ret = (ssize_t)enqueue_work_item_and_wait_for_result(work); + perf_end(_dm_read_perf); + return ret; +} + +/** Clear a data Item */ +__EXPORT int +dm_clear(dm_item_t item) +{ + work_q_item_t *work; + + /* Make sure data manager has been started and is not shutting down */ + if (!is_running() || g_task_should_exit) { + return -1; + } + + /* get a work item and queue up a clear request */ + if ((work = create_work_item()) == nullptr) { + return -1; + } + + work->func = dm_clear_func; + work->clear_params.item = item; + + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); +} + +__EXPORT int +dm_lock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if (!is_running() || g_task_should_exit) { + errno = EINVAL; + return -1; + } + + if (item >= DM_KEY_NUM_KEYS) { + errno = EINVAL; + return -1; + } + + if (g_item_locks[item]) { + return px4_sem_wait(g_item_locks[item]); + } + + errno = EINVAL; + return -1; +} + +__EXPORT int +dm_trylock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if (!is_running() || g_task_should_exit) { + errno = EINVAL; + return -1; + } + + if (item >= DM_KEY_NUM_KEYS) { + errno = EINVAL; + return -1; + } + + if (g_item_locks[item]) { + return px4_sem_trywait(g_item_locks[item]); + } + + errno = EINVAL; + return -1; +} + +/** Unlock a data Item */ +__EXPORT void +dm_unlock(dm_item_t item) +{ + /* Make sure data manager has been started and is not shutting down */ + if (!is_running() || g_task_should_exit) { + return; + } + + if (item >= DM_KEY_NUM_KEYS) { + return; + } + + if (g_item_locks[item]) { + px4_sem_post(g_item_locks[item]); + } +} + +/** Tell the data manager about the type of the last reset */ +__EXPORT int +dm_restart(dm_reset_reason reason) +{ + work_q_item_t *work; + + /* Make sure data manager has been started and is not shutting down */ + if (!is_running() || g_task_should_exit) { + return -1; + } + + /* get a work item and queue up a restart request */ + if ((work = create_work_item()) == nullptr) { + return -1; + } + + work->func = dm_restart_func; + work->restart_params.reason = reason; + + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); +} + +static int +task_main(int argc, char *argv[]) +{ + /* Dataman can use disk or RAM */ + switch (backend) { + case BACKEND_FILE: + g_dm_ops = &dm_file_operations; + break; + + case BACKEND_RAM: + g_dm_ops = &dm_ram_operations; + break; + + default: + PX4_WARN("No valid backend set."); + return -1; + } + + work_q_item_t *work; + + /* Initialize global variables */ + g_key_offsets[0] = 0; + + for (int i = 0; i < ((int)DM_KEY_NUM_KEYS - 1); i++) { + g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * g_per_item_size[i]); + } + + unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * + g_per_item_size[DM_KEY_NUM_KEYS - 1]); + + for (unsigned i = 0; i < dm_number_of_funcs; i++) { + g_func_counts[i] = 0; + } + + /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE & DM_KEY_FENCE_POINTS supports locking */ + px4_sem_init(&g_sys_state_mutex_mission, 1, 1); /* Initially unlocked */ + px4_sem_init(&g_sys_state_mutex_fence, 1, 1); /* Initially unlocked */ + + for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) { + g_item_locks[i] = nullptr; + } + + g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex_mission; + g_item_locks[DM_KEY_FENCE_POINTS] = &g_sys_state_mutex_fence; + + g_task_should_exit = false; + + init_q(&g_work_q); + init_q(&g_free_q); + + px4_sem_init(&g_work_queued_sema, 1, 0); + + /* g_work_queued_sema use case is a signal */ + + px4_sem_setprotocol(&g_work_queued_sema, SEM_PRIO_NONE); + + _dm_read_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": read"); + _dm_write_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": write"); + + /* see if we need to erase any items based on restart type */ + int32_t sys_restart_val; + + const char *restart_type_str = "Unknown restart"; + + int ret = g_dm_ops->initialize(max_offset); + + if (ret) { + g_task_should_exit = true; + goto end; + } + + if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { + if (sys_restart_val == DM_INIT_REASON_POWER_ON) { + restart_type_str = "Power on restart"; + g_dm_ops->restart(DM_INIT_REASON_POWER_ON); + + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + restart_type_str = "In flight restart"; + g_dm_ops->restart(DM_INIT_REASON_IN_FLIGHT); + } + } + + switch (backend) { + case BACKEND_FILE: + if (sys_restart_val != DM_INIT_REASON_POWER_ON) { + PX4_INFO("%s, data manager file '%s' size is %u bytes", + restart_type_str, k_data_manager_device_path, max_offset); + } + + break; + + case BACKEND_RAM: + PX4_INFO("%s, data manager RAM size is %u bytes", + restart_type_str, max_offset); + break; + + default: + break; + } + + /* Tell startup that the worker thread has completed its initialization */ + px4_sem_post(&g_init_sema); + + /* Start the endless loop, waiting for then processing work requests */ + while (true) { + + /* do we need to exit ??? */ + if (!g_task_should_exit) { + /* wait for work */ + g_dm_ops->wait(&g_work_queued_sema); + } + + /* Empty the work queue */ + while ((work = dequeue_work_item())) { + + /* handle each work item with the appropriate handler */ + switch (work->func) { + case dm_write_func: + g_func_counts[dm_write_func]++; + work->result = + g_dm_ops->write(work->write_params.item, work->write_params.index, work->write_params.persistence, + work->write_params.buf, + work->write_params.count); + break; + + case dm_read_func: + g_func_counts[dm_read_func]++; + work->result = + g_dm_ops->read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count); + break; + + case dm_clear_func: + g_func_counts[dm_clear_func]++; + work->result = g_dm_ops->clear(work->clear_params.item); + break; + + case dm_restart_func: + g_func_counts[dm_restart_func]++; + work->result = g_dm_ops->restart(work->restart_params.reason); + break; + + default: /* should never happen */ + work->result = -1; + break; + } + + /* Inform the caller that work is done */ + px4_sem_post(&work->wait_sem); + } + + /* time to go???? */ + if (g_task_should_exit) { + break; + } + } + + g_dm_ops->shutdown(); + + /* The work queue is now empty, empty the free queue */ + for (;;) { + if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == nullptr) { + break; + } + + if (work->first) { + free(work); + } + } + +end: + backend = BACKEND_NONE; + destroy_q(&g_work_q); + destroy_q(&g_free_q); + px4_sem_destroy(&g_work_queued_sema); + px4_sem_destroy(&g_sys_state_mutex_mission); + px4_sem_destroy(&g_sys_state_mutex_fence); + + perf_free(_dm_read_perf); + _dm_read_perf = nullptr; + + perf_free(_dm_write_perf); + _dm_write_perf = nullptr; + + return 0; +} + +static int +start() +{ + int task; + + px4_sem_init(&g_init_sema, 1, 0); + + /* g_init_sema use case is a signal */ + + px4_sem_setprotocol(&g_init_sema, SEM_PRIO_NONE); + + /* start the worker thread with low priority for disk IO */ + if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 10, TASK_STACK_SIZE, task_main, + nullptr)) < 0) { + px4_sem_destroy(&g_init_sema); + PX4_ERR("task start failed"); + return -1; + } + + /* wait for the thread to actually initialize */ + px4_sem_wait(&g_init_sema); + px4_sem_destroy(&g_init_sema); + + return 0; +} + +static void +status() +{ + /* display usage statistics */ + PX4_INFO("Writes %u", g_func_counts[dm_write_func]); + PX4_INFO("Reads %u", g_func_counts[dm_read_func]); + PX4_INFO("Clears %u", g_func_counts[dm_clear_func]); + PX4_INFO("Restarts %u", g_func_counts[dm_restart_func]); + PX4_INFO("Max Q lengths work %u, free %u", g_work_q.max_size, g_free_q.max_size); + perf_print_counter(_dm_read_perf); + perf_print_counter(_dm_write_perf); +} + +static void +stop() +{ + /* Tell the worker task to shut down */ + g_task_should_exit = true; + px4_sem_post(&g_work_queued_sema); +} + +static void +usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Module to provide persistent storage for the rest of the system in form of a simple database through a C API. +Multiple backends are supported: +- a file (eg. on the SD card) +- RAM (this is obviously not persistent) + +It is used to store structured data of different types: mission waypoints, mission state and geofence polygons. +Each type has a specific type and a fixed maximum amount of storage items, so that fast random access is possible. + +### Implementation +Reading and writing a single item is always atomic. If multiple items need to be read/modified atomically, there is +an additional lock per item type via `dm_lock`. + +**DM_KEY_FENCE_POINTS** and **DM_KEY_SAFE_POINTS** items: the first data element is a `mission_stats_entry_s` struct, +which stores the number of items for these types. These items are always updated atomically in one transaction (from +the mavlink mission manager). During that time, navigator will try to acquire the geofence item lock, fail, and will not +check for geofence violations. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("dataman", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('f', nullptr, "", "Storage file", true); + PRINT_MODULE_USAGE_PARAM_FLAG('r', "Use RAM backend (NOT persistent)", true); + PRINT_MODULE_USAGE_PARAM_COMMENT("The options -f and -r are mutually exclusive. If nothing is specified, a file 'dataman' is used"); + + PRINT_MODULE_USAGE_COMMAND_DESCR("poweronrestart", "Restart dataman (on power on)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("inflightrestart", "Restart dataman (in flight)"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +static int backend_check() +{ + if (backend != BACKEND_NONE) { + PX4_WARN("-f and -r are mutually exclusive"); + usage(); + return -1; + } + + return 0; +} + +int +dataman_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return -1; + } + + if (!strcmp(argv[1], "start")) { + + if (is_running()) { + PX4_WARN("dataman already running"); + return -1; + } + + int ch; + int dmoptind = 1; + const char *dmoptarg = nullptr; + + /* jump over start and look at options first */ + + while ((ch = px4_getopt(argc, argv, "f:r", &dmoptind, &dmoptarg)) != EOF) { + switch (ch) { + case 'f': + if (backend_check()) { + return -1; + } + + backend = BACKEND_FILE; + k_data_manager_device_path = strdup(dmoptarg); + PX4_INFO("dataman file set to: %s", k_data_manager_device_path); + break; + + case 'r': + if (backend_check()) { + return -1; + } + + backend = BACKEND_RAM; + break; + + //no break + default: + usage(); + return -1; + } + } + + if (backend == BACKEND_NONE) { + backend = BACKEND_FILE; + k_data_manager_device_path = strdup(default_device_path); + } + + start(); + + if (!is_running()) { + PX4_ERR("dataman start failed"); + free(k_data_manager_device_path); + k_data_manager_device_path = nullptr; + return -1; + } + + return 0; + } + + /* Worker thread should be running for all other commands */ + if (!is_running()) { + PX4_WARN("dataman worker thread not running"); + usage(); + return -1; + } + + if (!strcmp(argv[1], "stop")) { + stop(); + free(k_data_manager_device_path); + k_data_manager_device_path = nullptr; + + } else if (!strcmp(argv[1], "status")) { + status(); + + } else if (!strcmp(argv[1], "poweronrestart")) { + dm_restart(DM_INIT_REASON_POWER_ON); + + } else if (!strcmp(argv[1], "inflightrestart")) { + dm_restart(DM_INIT_REASON_IN_FLIGHT); + + } else { + usage(); + return -1; + } + + return 0; +} diff --git a/src/prometheus_px4/src/modules/dataman/dataman.h b/src/prometheus_px4/src/modules/dataman/dataman.h new file mode 100644 index 0000000..c7bb0b0 --- /dev/null +++ b/src/prometheus_px4/src/modules/dataman/dataman.h @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * 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 dataman.h + * + * DATAMANAGER driver. + */ +#pragma once + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** Types of items that the data manager can store */ +typedef enum { + DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ + DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ + DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alternate between 0 and 1) */ + DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_COMPAT, + DM_KEY_NUM_KEYS /* Total number of item types defined */ +} dm_item_t; + +#if defined(MEMORY_CONSTRAINED_SYSTEM) +enum { + DM_KEY_SAFE_POINTS_MAX = 8, + DM_KEY_FENCE_POINTS_MAX = 16, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_ONBOARD_MAX = (NUM_MISSIONS_SUPPORTED / 10), + DM_KEY_MISSION_STATE_MAX = 1, + DM_KEY_COMPAT_MAX = 1 +}; +#else +/** The maximum number of instances for each item type */ +enum { + DM_KEY_SAFE_POINTS_MAX = 8, + DM_KEY_FENCE_POINTS_MAX = 64, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_MISSION_STATE_MAX = 1, + DM_KEY_COMPAT_MAX = 1 +}; +#endif +/** Data persistence levels */ +typedef enum { + DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ + DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ + DM_PERSIST_VOLATILE /* Data does not survive resets */ +} dm_persitence_t; + +/** The reason for the last reset */ +typedef enum { + DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ + DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ + DM_INIT_REASON_VOLATILE /* Data does not survive reset */ +} dm_reset_reason; + +struct dataman_compat_s { + uint64_t key; +}; + +/* increment this define whenever a binary incompatible change is performed */ +#define DM_COMPAT_VERSION 2ULL + +#define DM_COMPAT_KEY ((DM_COMPAT_VERSION << 32) + (sizeof(struct mission_item_s) << 24) + \ + (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \ + (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_safe_point_s) << 4) + \ + sizeof(struct dataman_compat_s)) + +/** Retrieve from the data manager store */ +__EXPORT ssize_t +dm_read( + dm_item_t item, /* The item type to retrieve */ + unsigned index, /* The index of the item */ + void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ +); + +/** write to the data manager store */ +__EXPORT ssize_t +dm_write( + dm_item_t item, /* The item type to store */ + unsigned index, /* The index of the item */ + dm_persitence_t persistence, /* The persistence level of this item */ + const void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ +); + +/** + * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated + * atomically). + * Note that this lock is independent from dm_read & dm_write calls. + * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) + */ +__EXPORT int +dm_lock( + dm_item_t item /* The item type to lock */ +); + +/** + * Try to lock all items of a type (@see sem_trywait()). + * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) + */ +__EXPORT int +dm_trylock( + dm_item_t item /* The item type to lock */ +); + +/** Unlock all items of a type */ +__EXPORT void +dm_unlock( + dm_item_t item /* The item type to unlock */ +); + +/** Erase all items of this type */ +__EXPORT int +dm_clear( + dm_item_t item /* The item type to clear */ +); + +/** Tell the data manager about the type of the last reset */ +__EXPORT int +dm_restart( + dm_reset_reason restart_type /* The last reset type */ +); + +#ifdef __cplusplus +} +#endif diff --git a/src/prometheus_px4/src/modules/ekf2/CMakeLists.txt b/src/prometheus_px4/src/modules/ekf2/CMakeLists.txt new file mode 100644 index 0000000..56d0b14 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/CMakeLists.txt @@ -0,0 +1,53 @@ +############################################################################ +# +# 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. +# +############################################################################# + +add_subdirectory(Utility) + +px4_add_module( + MODULE modules__ekf2 + MAIN ekf2 + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + EKF2.cpp + EKF2.hpp + EKF2Selector.cpp + EKF2Selector.hpp + DEPENDS + git_ecl + ecl_EKF + ecl_geo + perf + EKF2Utility + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/ekf2/EKF2.cpp b/src/prometheus_px4/src/modules/ekf2/EKF2.cpp new file mode 100644 index 0000000..716d6e4 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/EKF2.cpp @@ -0,0 +1,2027 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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 "EKF2.hpp" + +using namespace time_literals; +using math::constrain; +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Vector3f; + +pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER; +static px4::atomic _objects[EKF2_MAX_INSTANCES] {}; +#if !defined(CONSTRAINED_FLASH) +static px4::atomic _ekf2_selector {nullptr}; +#endif // !CONSTRAINED_FLASH + +EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, config), + _replay_mode(replay_mode && !multi_mode), + _multi_mode(multi_mode), + _instance(multi_mode ? -1 : 0), + _attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)), + _local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)), + _global_position_pub(multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)), + _odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)), + _wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)), + _params(_ekf.getParamHandle()), + _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms), + _param_ekf2_mag_delay(_params->mag_delay_ms), + _param_ekf2_baro_delay(_params->baro_delay_ms), + _param_ekf2_gps_delay(_params->gps_delay_ms), + _param_ekf2_of_delay(_params->flow_delay_ms), + _param_ekf2_rng_delay(_params->range_delay_ms), + _param_ekf2_asp_delay(_params->airspeed_delay_ms), + _param_ekf2_ev_delay(_params->ev_delay_ms), + _param_ekf2_avel_delay(_params->auxvel_delay_ms), + _param_ekf2_gyr_noise(_params->gyro_noise), + _param_ekf2_acc_noise(_params->accel_noise), + _param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise), + _param_ekf2_acc_b_noise(_params->accel_bias_p_noise), + _param_ekf2_mag_e_noise(_params->mage_p_noise), + _param_ekf2_mag_b_noise(_params->magb_p_noise), + _param_ekf2_wind_noise(_params->wind_vel_p_noise), + _param_ekf2_terr_noise(_params->terrain_p_noise), + _param_ekf2_terr_grad(_params->terrain_gradient), + _param_ekf2_gps_v_noise(_params->gps_vel_noise), + _param_ekf2_gps_p_noise(_params->gps_pos_noise), + _param_ekf2_noaid_noise(_params->pos_noaid_noise), + _param_ekf2_baro_noise(_params->baro_noise), + _param_ekf2_baro_gate(_params->baro_innov_gate), + _param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone), + _param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt), + _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate), + _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate), + _param_ekf2_tas_gate(_params->tas_innov_gate), + _param_ekf2_head_noise(_params->mag_heading_noise), + _param_ekf2_mag_noise(_params->mag_noise), + _param_ekf2_eas_noise(_params->eas_noise), + _param_ekf2_beta_gate(_params->beta_innov_gate), + _param_ekf2_beta_noise(_params->beta_noise), + _param_ekf2_mag_decl(_params->mag_declination_deg), + _param_ekf2_hdg_gate(_params->heading_innov_gate), + _param_ekf2_mag_gate(_params->mag_innov_gate), + _param_ekf2_decl_type(_params->mag_declination_source), + _param_ekf2_mag_type(_params->mag_fusion_type), + _param_ekf2_mag_acclim(_params->mag_acc_gate), + _param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate), + _param_ekf2_gps_check(_params->gps_check_mask), + _param_ekf2_req_eph(_params->req_hacc), + _param_ekf2_req_epv(_params->req_vacc), + _param_ekf2_req_sacc(_params->req_sacc), + _param_ekf2_req_nsats(_params->req_nsats), + _param_ekf2_req_pdop(_params->req_pdop), + _param_ekf2_req_hdrift(_params->req_hdrift), + _param_ekf2_req_vdrift(_params->req_vdrift), + _param_ekf2_aid_mask(_params->fusion_mode), + _param_ekf2_hgt_mode(_params->vdist_sensor_type), + _param_ekf2_terr_mask(_params->terrain_fusion_mode), + _param_ekf2_noaid_tout(_params->valid_timeout_max), + _param_ekf2_rng_noise(_params->range_noise), + _param_ekf2_rng_sfe(_params->range_noise_scaler), + _param_ekf2_rng_gate(_params->range_innov_gate), + _param_ekf2_min_rng(_params->rng_gnd_clearance), + _param_ekf2_rng_pitch(_params->rng_sens_pitch), + _param_ekf2_rng_aid(_params->range_aid), + _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), + _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), + _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), + _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), + _param_ekf2_evv_gate(_params->ev_vel_innov_gate), + _param_ekf2_evp_gate(_params->ev_pos_innov_gate), + _param_ekf2_of_n_min(_params->flow_noise), + _param_ekf2_of_n_max(_params->flow_noise_qual_min), + _param_ekf2_of_qmin(_params->flow_qual_min), + _param_ekf2_of_gate(_params->flow_innov_gate), + _param_ekf2_imu_pos_x(_params->imu_pos_body(0)), + _param_ekf2_imu_pos_y(_params->imu_pos_body(1)), + _param_ekf2_imu_pos_z(_params->imu_pos_body(2)), + _param_ekf2_gps_pos_x(_params->gps_pos_body(0)), + _param_ekf2_gps_pos_y(_params->gps_pos_body(1)), + _param_ekf2_gps_pos_z(_params->gps_pos_body(2)), + _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), + _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), + _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), + _param_ekf2_of_pos_x(_params->flow_pos_body(0)), + _param_ekf2_of_pos_y(_params->flow_pos_body(1)), + _param_ekf2_of_pos_z(_params->flow_pos_body(2)), + _param_ekf2_ev_pos_x(_params->ev_pos_body(0)), + _param_ekf2_ev_pos_y(_params->ev_pos_body(1)), + _param_ekf2_ev_pos_z(_params->ev_pos_body(2)), + _param_ekf2_tau_vel(_params->vel_Tau), + _param_ekf2_tau_pos(_params->pos_Tau), + _param_ekf2_gbias_init(_params->switch_on_gyro_bias), + _param_ekf2_abias_init(_params->switch_on_accel_bias), + _param_ekf2_angerr_init(_params->initial_tilt_err), + _param_ekf2_abl_lim(_params->acc_bias_lim), + _param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim), + _param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim), + _param_ekf2_abl_tau(_params->acc_bias_learn_tc), + _param_ekf2_drag_noise(_params->drag_noise), + _param_ekf2_bcoef_x(_params->bcoef_x), + _param_ekf2_bcoef_y(_params->bcoef_y), + _param_ekf2_aspd_max(_params->max_correction_airspeed), + _param_ekf2_pcoef_xp(_params->static_pressure_coef_xp), + _param_ekf2_pcoef_xn(_params->static_pressure_coef_xn), + _param_ekf2_pcoef_yp(_params->static_pressure_coef_yp), + _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn), + _param_ekf2_pcoef_z(_params->static_pressure_coef_z), + _param_ekf2_move_test(_params->is_moving_scaler), + _param_ekf2_mag_check(_params->check_mag_strength), + _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), + _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) +{ +} + +EKF2::~EKF2() +{ + perf_free(_ecl_ekf_update_perf); + perf_free(_ecl_ekf_update_full_perf); + perf_free(_msg_missed_imu_perf); + perf_free(_msg_missed_air_data_perf); + perf_free(_msg_missed_airspeed_perf); + perf_free(_msg_missed_distance_sensor_perf); + perf_free(_msg_missed_gps_perf); + perf_free(_msg_missed_landing_target_pose_perf); + perf_free(_msg_missed_magnetometer_perf); + perf_free(_msg_missed_odometry_perf); + perf_free(_msg_missed_optical_flow_perf); +} + +bool EKF2::multi_init(int imu, int mag) +{ + // advertise immediately to ensure consistent uORB instance numbering + _attitude_pub.advertise(); + _local_position_pub.advertise(); + _global_position_pub.advertise(); + _odometry_pub.advertise(); + _wind_pub.advertise(); + + _ekf2_timestamps_pub.advertise(); + _ekf_gps_drift_pub.advertise(); + _estimator_innovation_test_ratios_pub.advertise(); + _estimator_innovation_variances_pub.advertise(); + _estimator_innovations_pub.advertise(); + _estimator_optical_flow_vel_pub.advertise(); + _estimator_sensor_bias_pub.advertise(); + _estimator_states_pub.advertise(); + _estimator_status_pub.advertise(); + _estimator_status_flags_pub.advertise(); + _estimator_visual_odometry_aligned_pub.advertised(); + _yaw_est_pub.advertise(); + + bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); + + const int status_instance = _estimator_states_pub.get_instance(); + + if ((status_instance >= 0) && changed_instance + && (_attitude_pub.get_instance() == status_instance) + && (_local_position_pub.get_instance() == status_instance) + && (_global_position_pub.get_instance() == status_instance)) { + + _instance = status_instance; + + ScheduleNow(); + return true; + } + + PX4_ERR("publication instance problem: %d att: %d lpos: %d gpos: %d", status_instance, + _attitude_pub.get_instance(), _local_position_pub.get_instance(), _global_position_pub.get_instance()); + + return false; +} + +int EKF2::print_status() +{ + PX4_INFO_RAW("ekf2:%d attitude: %d, local position: %d, global position: %d\n", _instance, _ekf.attitude_valid(), + _ekf.local_position_is_valid(), _ekf.global_position_is_valid()); + perf_print_counter(_ecl_ekf_update_perf); + perf_print_counter(_ecl_ekf_update_full_perf); + perf_print_counter(_msg_missed_imu_perf); + perf_print_counter(_msg_missed_air_data_perf); + perf_print_counter(_msg_missed_airspeed_perf); + perf_print_counter(_msg_missed_distance_sensor_perf); + perf_print_counter(_msg_missed_gps_perf); + perf_print_counter(_msg_missed_landing_target_pose_perf); + perf_print_counter(_msg_missed_magnetometer_perf); + perf_print_counter(_msg_missed_odometry_perf); + perf_print_counter(_msg_missed_optical_flow_perf); + + return 0; +} + +void EKF2::Run() +{ + if (should_exit()) { + _sensor_combined_sub.unregisterCallback(); + _vehicle_imu_sub.unregisterCallback(); + + return; + } + + // check for parameter updates + if (_parameter_update_sub.updated() || !_callback_registered) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + + _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); + + // The airspeed scale factor correcton is only available via parameter as used by the airspeed module + param_t param_aspd_scale = param_find("ASPD_SCALE"); + + if (param_aspd_scale != PARAM_INVALID) { + param_get(param_aspd_scale, &_airspeed_scale_factor); + } + } + + if (!_callback_registered) { + if (_multi_mode) { + _callback_registered = _vehicle_imu_sub.registerCallback(); + + } else { + _callback_registered = _sensor_combined_sub.registerCallback(); + } + + if (!_callback_registered) { + PX4_WARN("%d - failed to register callback, retrying", _instance); + ScheduleDelayed(1_s); + return; + } + } + + if (_vehicle_command_sub.updated()) { + vehicle_command_s vehicle_command; + + if (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) { + if (!_ekf.control_status_flags().in_air) { + + uint64_t origin_time {}; + double latitude = vehicle_command.param5; + double longitude = vehicle_command.param6; + float altitude = vehicle_command.param7; + + _ekf.setEkfGlobalOrigin(latitude, longitude, altitude); + + // Validate the ekf origin status. + _ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude); + PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast(altitude)); + } + } + } + } + + bool imu_updated = false; + imuSample imu_sample_new {}; + + hrt_abstime imu_dt = 0; // for tracking time slip later + + if (_multi_mode) { + const unsigned last_generation = _vehicle_imu_sub.get_last_generation(); + vehicle_imu_s imu; + imu_updated = _vehicle_imu_sub.update(&imu); + + if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) { + perf_count(_msg_missed_imu_perf); + } + + imu_sample_new.time_us = imu.timestamp_sample; + imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f; + imu_sample_new.delta_ang = Vector3f{imu.delta_angle}; + imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f; + imu_sample_new.delta_vel = Vector3f{imu.delta_velocity}; + + if (imu.delta_velocity_clipping > 0) { + imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X; + imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y; + imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z; + } + + imu_dt = imu.delta_angle_dt; + + if ((_device_id_accel == 0) || (_device_id_gyro == 0)) { + _device_id_accel = imu.accel_device_id; + _device_id_gyro = imu.gyro_device_id; + _imu_calibration_count = imu.calibration_count; + + } else if ((imu.calibration_count > _imu_calibration_count) + || (imu.accel_device_id != _device_id_accel) + || (imu.gyro_device_id != _device_id_gyro)) { + + PX4_INFO("%d - resetting IMU bias", _instance); + _device_id_accel = imu.accel_device_id; + _device_id_gyro = imu.gyro_device_id; + + _ekf.resetImuBias(); + _imu_calibration_count = imu.calibration_count; + } + + } else { + const unsigned last_generation = _sensor_combined_sub.get_last_generation(); + sensor_combined_s sensor_combined; + imu_updated = _sensor_combined_sub.update(&sensor_combined); + + if (imu_updated && (_sensor_combined_sub.get_last_generation() != last_generation + 1)) { + perf_count(_msg_missed_imu_perf); + } + + imu_sample_new.time_us = sensor_combined.timestamp; + imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f; + imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt; + imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f; + imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt; + + if (sensor_combined.accelerometer_clipping > 0) { + imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X; + imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y; + imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z; + } + + imu_dt = sensor_combined.gyro_integral_dt; + + if (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0)) { + sensor_selection_s sensor_selection; + + if (_sensor_selection_sub.copy(&sensor_selection)) { + if (_device_id_accel != sensor_selection.accel_device_id) { + _ekf.resetAccelBias(); + _device_id_accel = sensor_selection.accel_device_id; + } + + if (_device_id_gyro != sensor_selection.gyro_device_id) { + _ekf.resetGyroBias(); + _device_id_gyro = sensor_selection.gyro_device_id; + } + } + } + } + + if (imu_updated) { + const hrt_abstime now = imu_sample_new.time_us; + + // push imu data into estimator + _ekf.setIMUData(imu_sample_new); + PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor) + + // integrate time to monitor time slippage + if (_start_time_us > 0) { + _integrated_time_us += imu_dt; + _last_time_slip_us = (imu_sample_new.time_us - _start_time_us) - _integrated_time_us; + + } else { + _start_time_us = imu_sample_new.time_us; + _last_time_slip_us = 0; + } + + // update all other topics if they have new data + if (_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_status_sub.copy(&vehicle_status)) { + const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + + // only fuse synthetic sideslip measurements if conditions are met + _ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1)); + + // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) + _ekf.set_is_fixed_wing(is_fixed_wing); + + _preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type != + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + // update standby (arming state) flag + const bool standby = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY); + + if (_standby != standby) { + _standby = standby; + + // reset preflight checks if transitioning in or out of standby arming state + _preflt_checker.reset(); + } + } + } + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _ekf.set_in_air_status(!vehicle_land_detected.landed); + + if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) { + if (!_had_valid_terrain) { + // update ground effect flag based on land detector state if we've never had valid terrain data + _ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect); + } + + } else { + _ekf.set_gnd_effect_flag(false); + } + } + } + + // ekf2_timestamps (using 0.1 ms relative timestamps) + ekf2_timestamps_s ekf2_timestamps { + .timestamp = now, + .airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .vehicle_magnetometer_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + }; + + UpdateAirspeedSample(ekf2_timestamps); + UpdateAuxVelSample(ekf2_timestamps); + UpdateBaroSample(ekf2_timestamps); + UpdateGpsSample(ekf2_timestamps); + UpdateMagSample(ekf2_timestamps); + UpdateRangeSample(ekf2_timestamps); + + vehicle_odometry_s ev_odom; + const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom); + + optical_flow_s optical_flow; + const bool new_optical_flow = UpdateFlowSample(ekf2_timestamps, optical_flow); + + + // run the EKF update and output + const hrt_abstime ekf_update_start = hrt_absolute_time(); + + if (_ekf.update()) { + perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start)); + + PublishLocalPosition(now); + PublishOdometry(now, imu_sample_new); + PublishGlobalPosition(now); + PublishSensorBias(now); + PublishWindEstimate(now); + + // publish status/logging messages + PublishEkfDriftMetrics(now); + PublishEventFlags(now); + PublishStates(now); + PublishStatus(now); + PublishStatusFlags(now); + PublishInnovations(now, imu_sample_new); + PublishInnovationTestRatios(now); + PublishInnovationVariances(now); + PublishYawEstimatorStatus(now); + + UpdateMagCalibration(now); + + } else { + // ekf no update + perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start)); + } + + // publish external visual odometry after fixed frame alignment if new odometry is received + if (new_ev_odom) { + PublishOdometryAligned(now, ev_odom); + } + + if (new_optical_flow) { + PublishOpticalFlowVel(now, optical_flow); + } + + // publish ekf2_timestamps + _ekf2_timestamps_pub.publish(ekf2_timestamps); + } +} + +void EKF2::PublishAttitude(const hrt_abstime ×tamp) +{ + if (_ekf.attitude_valid()) { + // generate vehicle attitude quaternion data + vehicle_attitude_s att; + att.timestamp_sample = timestamp; + const Quatf q{_ekf.calculate_quaternion()}; + q.copyTo(att.q); + + _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); + att.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _attitude_pub.publish(att); + + } else if (_replay_mode) { + // in replay mode we have to tell the replay module not to wait for an update + // we do this by publishing an attitude with zero timestamp + vehicle_attitude_s att{}; + _attitude_pub.publish(att); + } +} + +void EKF2::PublishEkfDriftMetrics(const hrt_abstime ×tamp) +{ + // publish GPS drift data only when updated to minimise overhead + float gps_drift[3]; + bool blocked; + + if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) { + ekf_gps_drift_s drift_data; + drift_data.hpos_drift_rate = gps_drift[0]; + drift_data.vpos_drift_rate = gps_drift[1]; + drift_data.hspd = gps_drift[2]; + drift_data.blocked = blocked; + drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + + _ekf_gps_drift_pub.publish(drift_data); + } +} + +void EKF2::PublishEventFlags(const hrt_abstime ×tamp) +{ + // information events + uint32_t information_events = _ekf.information_event_status().value; + bool information_event_updated = false; + + if (information_events != 0) { + information_event_updated = true; + _filter_information_event_changes++; + } + + // warning events + uint32_t warning_events = _ekf.warning_event_status().value; + bool warning_event_updated = false; + + if (warning_events != 0) { + warning_event_updated = true; + _filter_warning_event_changes++; + } + + if (information_event_updated || warning_event_updated) { + estimator_event_flags_s event_flags{}; + event_flags.timestamp_sample = timestamp; + + event_flags.information_event_changes = _filter_information_event_changes; + event_flags.gps_checks_passed = _ekf.information_event_flags().gps_checks_passed; + event_flags.reset_vel_to_gps = _ekf.information_event_flags().reset_vel_to_gps; + event_flags.reset_vel_to_flow = _ekf.information_event_flags().reset_vel_to_flow; + event_flags.reset_vel_to_vision = _ekf.information_event_flags().reset_vel_to_vision; + event_flags.reset_vel_to_zero = _ekf.information_event_flags().reset_vel_to_zero; + event_flags.reset_pos_to_last_known = _ekf.information_event_flags().reset_pos_to_last_known; + event_flags.reset_pos_to_gps = _ekf.information_event_flags().reset_pos_to_gps; + event_flags.reset_pos_to_vision = _ekf.information_event_flags().reset_pos_to_vision; + event_flags.starting_gps_fusion = _ekf.information_event_flags().starting_gps_fusion; + event_flags.starting_vision_pos_fusion = _ekf.information_event_flags().starting_vision_pos_fusion; + event_flags.starting_vision_vel_fusion = _ekf.information_event_flags().starting_vision_vel_fusion; + event_flags.starting_vision_yaw_fusion = _ekf.information_event_flags().starting_vision_yaw_fusion; + event_flags.yaw_aligned_to_imu_gps = _ekf.information_event_flags().yaw_aligned_to_imu_gps; + + event_flags.warning_event_changes = _filter_warning_event_changes; + event_flags.gps_quality_poor = _ekf.warning_event_flags().gps_quality_poor; + event_flags.gps_fusion_timout = _ekf.warning_event_flags().gps_fusion_timout; + event_flags.gps_data_stopped = _ekf.warning_event_flags().gps_data_stopped; + event_flags.gps_data_stopped_using_alternate = _ekf.warning_event_flags().gps_data_stopped_using_alternate; + event_flags.height_sensor_timeout = _ekf.warning_event_flags().height_sensor_timeout; + event_flags.stopping_navigation = _ekf.warning_event_flags().stopping_mag_use; + event_flags.invalid_accel_bias_cov_reset = _ekf.warning_event_flags().invalid_accel_bias_cov_reset; + event_flags.bad_yaw_using_gps_course = _ekf.warning_event_flags().bad_yaw_using_gps_course; + event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use; + event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped; + event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped; + + event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_event_flags_pub.publish(event_flags); + } + + _ekf.clear_information_events(); + _ekf.clear_warning_events(); +} + +void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) +{ + if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { + const Vector3f position{_ekf.getPosition()}; + + // generate and publish global position data + vehicle_global_position_s global_pos; + global_pos.timestamp_sample = timestamp; + + // Position of local NED origin in GPS / WGS84 frame + map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon); + + float delta_xy[2]; + _ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter); + + global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters + global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); + + // global altitude has opposite sign of local down position + float delta_z; + uint8_t z_reset_counter; + _ekf.get_posD_reset(&delta_z, &z_reset_counter); + global_pos.delta_alt = -delta_z; + + _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); + + if (_ekf.isTerrainEstimateValid()) { + // Terrain altitude in m, WGS84 + global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); + global_pos.terrain_alt_valid = true; + + } else { + global_pos.terrain_alt = NAN; + global_pos.terrain_alt_valid = false; + } + + global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning + global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _global_position_pub.publish(global_pos); + } +} + +void EKF2::PublishInnovations(const hrt_abstime ×tamp, const imuSample &imu) +{ + // publish estimator innovation data + estimator_innovations_s innovations{}; + innovations.timestamp_sample = timestamp; + _ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos); + _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos); + _ekf.getBaroHgtInnov(innovations.baro_vpos); + _ekf.getRngHgtInnov(innovations.rng_vpos); + _ekf.getAuxVelInnov(innovations.aux_hvel); + _ekf.getFlowInnov(innovations.flow); + _ekf.getHeadingInnov(innovations.heading); + _ekf.getMagInnov(innovations.mag_field); + _ekf.getDragInnov(innovations.drag); + _ekf.getAirspeedInnov(innovations.airspeed); + _ekf.getBetaInnov(innovations.beta); + _ekf.getHaglInnov(innovations.hagl); + // Not yet supported + innovations.aux_vvel = NAN; + + innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_innovations_pub.publish(innovations); + + // calculate noise filtered velocity innovations which are used for pre-flight checking + if (_standby) { + // TODO: move to run before publications + _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); + _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); + _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); + _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel); + + _preflt_checker.update(imu.delta_ang_dt, innovations); + } +} + +void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) +{ + // publish estimator innovation test ratio data + estimator_innovations_s test_ratios{}; + test_ratios.timestamp_sample = timestamp; + _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0], + test_ratios.gps_vpos); + _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos); + _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos); + _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); + _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); + _ekf.getFlowInnovRatio(test_ratios.flow[0]); + _ekf.getHeadingInnovRatio(test_ratios.heading); + _ekf.getMagInnovRatio(test_ratios.mag_field[0]); + _ekf.getDragInnovRatio(&test_ratios.drag[0]); + _ekf.getAirspeedInnovRatio(test_ratios.airspeed); + _ekf.getBetaInnovRatio(test_ratios.beta); + _ekf.getHaglInnovRatio(test_ratios.hagl); + // Not yet supported + test_ratios.aux_vvel = NAN; + + test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_innovation_test_ratios_pub.publish(test_ratios); +} + +void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) +{ + // publish estimator innovation variance data + estimator_innovations_s variances{}; + variances.timestamp_sample = timestamp; + _ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos); + _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos); + _ekf.getBaroHgtInnovVar(variances.baro_vpos); + _ekf.getRngHgtInnovVar(variances.rng_vpos); + _ekf.getAuxVelInnovVar(variances.aux_hvel); + _ekf.getFlowInnovVar(variances.flow); + _ekf.getHeadingInnovVar(variances.heading); + _ekf.getMagInnovVar(variances.mag_field); + _ekf.getDragInnovVar(variances.drag); + _ekf.getAirspeedInnovVar(variances.airspeed); + _ekf.getBetaInnovVar(variances.beta); + _ekf.getHaglInnovVar(variances.hagl); + // Not yet supported + variances.aux_vvel = NAN; + + variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_innovation_variances_pub.publish(variances); +} + +void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) +{ + vehicle_local_position_s lpos; + // generate vehicle local position data + lpos.timestamp_sample = timestamp; + + // Position of body origin in local NED frame + const Vector3f position{_ekf.getPosition()}; + lpos.x = position(0); + lpos.y = position(1); + lpos.z = position(2); + + // Velocity of body origin in local NED frame (m/s) + const Vector3f velocity{_ekf.getVelocity()}; + lpos.vx = velocity(0); + lpos.vy = velocity(1); + lpos.vz = velocity(2); + + // vertical position time derivative (m/s) + lpos.z_deriv = _ekf.getVerticalPositionDerivative(); + + // Acceleration of body origin in local frame + const Vector3f vel_deriv{_ekf.getVelocityDerivative()}; + lpos.ax = vel_deriv(0); + lpos.ay = vel_deriv(1); + lpos.az = vel_deriv(2); + + // TODO: better status reporting + lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed(); + lpos.z_valid = !_preflt_checker.hasVertFailed(); + lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed(); + lpos.v_z_valid = !_preflt_checker.hasVertFailed(); + + // Position of local NED origin in GPS / WGS84 frame + if (_ekf.global_origin_valid()) { + lpos.ref_timestamp = _ekf.global_origin().timestamp; + lpos.ref_lat = math::degrees(_ekf.global_origin().lat_rad); // Reference point latitude in degrees + lpos.ref_lon = math::degrees(_ekf.global_origin().lon_rad); // Reference point longitude in degrees + lpos.ref_alt = _ekf.getEkfGlobalOriginAltitude(); // Reference point in MSL altitude meters + lpos.xy_global = true; + lpos.z_global = true; + + } else { + lpos.ref_timestamp = 0; + lpos.ref_lat = static_cast(NAN); + lpos.ref_lon = static_cast(NAN); + lpos.ref_alt = NAN; + lpos.xy_global = false; + lpos.z_global = false; + } + + Quatf delta_q_reset; + _ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter); + + lpos.heading = Eulerf(_ekf.getQuaternion()).psi(); + lpos.delta_heading = Eulerf(delta_q_reset).psi(); + + // Distance to bottom surface (ground) in meters + // constrain the distance to ground to _rng_gnd_clearance + lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get()); + lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); + lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); + + if (!_had_valid_terrain) { + _had_valid_terrain = lpos.dist_bottom_valid; + } + + // only consider ground effect if compensation is configured and the vehicle is armed (props spinning) + if ((_param_ekf2_gnd_eff_dz.get() > 0.0f) && _armed && lpos.dist_bottom_valid) { + // set ground effect flag if vehicle is closer than a specified distance to the ground + _ekf.set_gnd_effect_flag(lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get()); + + // if we have no valid terrain estimate and never had one then use ground effect flag from land detector + // _had_valid_terrain is used to make sure that we don't fall back to using this option + // if we temporarily lose terrain data due to the distance sensor getting out of range + } + + _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); + _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); + + // get state reset information of position and velocity + _ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter); + _ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter); + _ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter); + _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter); + + // get control limit information + _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); + + // convert NaN to INFINITY + if (!PX4_ISFINITE(lpos.vxy_max)) { + lpos.vxy_max = INFINITY; + } + + if (!PX4_ISFINITE(lpos.vz_max)) { + lpos.vz_max = INFINITY; + } + + if (!PX4_ISFINITE(lpos.hagl_min)) { + lpos.hagl_min = INFINITY; + } + + if (!PX4_ISFINITE(lpos.hagl_max)) { + lpos.hagl_max = INFINITY; + } + + // publish vehicle local position data + lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _local_position_pub.publish(lpos); +} + +void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu) +{ + // generate vehicle odometry data + vehicle_odometry_s odom; + odom.timestamp_sample = imu.time_us; + + odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + // Vehicle odometry position + const Vector3f position{_ekf.getPosition()}; + odom.x = position(0); + odom.y = position(1); + odom.z = position(2); + + // Vehicle odometry linear velocity + odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; + const Vector3f velocity{_ekf.getVelocity()}; + odom.vx = velocity(0); + odom.vy = velocity(1); + odom.vz = velocity(2); + + // Vehicle odometry quaternion + _ekf.getQuaternion().copyTo(odom.q); + + // Vehicle odometry angular rates + const Vector3f gyro_bias{_ekf.getGyroBias()}; + const Vector3f rates{imu.delta_ang / imu.delta_ang_dt}; + odom.rollspeed = rates(0) - gyro_bias(0); + odom.pitchspeed = rates(1) - gyro_bias(1); + odom.yawspeed = rates(2) - gyro_bias(2); + + // get the covariance matrix size + static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]); + static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]); + + // Get covariances to vehicle odometry + float covariances[24]; + _ekf.covariances_diagonal().copyTo(covariances); + + // initially set pose covariances to 0 + for (size_t i = 0; i < POS_URT_SIZE; i++) { + odom.pose_covariance[i] = 0.0; + } + + // set the position variances + odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7]; + odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8]; + odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9]; + + // TODO: implement propagation from quaternion covariance to Euler angle covariance + // by employing the covariance law + + // initially set velocity covariances to 0 + for (size_t i = 0; i < VEL_URT_SIZE; i++) { + odom.velocity_covariance[i] = 0.0; + } + + // set the linear velocity variances + odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4]; + odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5]; + odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6]; + + // publish vehicle odometry data + odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _odometry_pub.publish(odom); +} + +void EKF2::PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom) +{ + const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame + const Dcmf ev_rot_mat(quat_ev2ekf); + + vehicle_odometry_s aligned_ev_odom{ev_odom}; + + // Rotate external position and velocity into EKF navigation frame + const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.x, ev_odom.y, ev_odom.z); + aligned_ev_odom.x = aligned_pos(0); + aligned_ev_odom.y = aligned_pos(1); + aligned_ev_odom.z = aligned_pos(2); + + switch (ev_odom.velocity_frame) { + case vehicle_odometry_s::BODY_FRAME_FRD: { + const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz); + aligned_ev_odom.vx = aligned_vel(0); + aligned_ev_odom.vy = aligned_vel(1); + aligned_ev_odom.vz = aligned_vel(2); + break; + } + + case vehicle_odometry_s::LOCAL_FRAME_FRD: { + const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz); + aligned_ev_odom.vx = aligned_vel(0); + aligned_ev_odom.vy = aligned_vel(1); + aligned_ev_odom.vz = aligned_vel(2); + break; + } + } + + aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + // Compute orientation in EKF navigation frame + Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q) ; + ev_quat_aligned.normalize(); + + ev_quat_aligned.copyTo(aligned_ev_odom.q); + quat_ev2ekf.copyTo(aligned_ev_odom.q_offset); + + _estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom); +} + +void EKF2::PublishSensorBias(const hrt_abstime ×tamp) +{ + // estimator_sensor_bias + estimator_sensor_bias_s bias{}; + bias.timestamp_sample = timestamp; + + const Vector3f gyro_bias{_ekf.getGyroBias()}; + const Vector3f accel_bias{_ekf.getAccelBias()}; + const Vector3f mag_bias{_mag_cal_last_bias}; + + // only publish on change + if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f) + || (accel_bias - _last_accel_bias_published).longerThan(0.001f) + || (mag_bias - _last_mag_bias_published).longerThan(0.001f)) { + + // take device ids from sensor_selection_s if not using specific vehicle_imu_s + if (_device_id_gyro != 0) { + bias.gyro_device_id = _device_id_gyro; + gyro_bias.copyTo(bias.gyro_bias); + bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates() + _ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance); + bias.gyro_bias_valid = true; + + _last_gyro_bias_published = gyro_bias; + } + + if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) { + bias.accel_device_id = _device_id_accel; + accel_bias.copyTo(bias.accel_bias); + bias.accel_bias_limit = _params->acc_bias_lim; + _ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance); + bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias; + + _last_accel_bias_published = accel_bias; + } + + if (_device_id_mag != 0) { + bias.mag_device_id = _device_id_mag; + mag_bias.copyTo(bias.mag_bias); + bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates() + _mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance); + bias.mag_bias_valid = _mag_cal_available; + + _last_mag_bias_published = mag_bias; + } + + bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_sensor_bias_pub.publish(bias); + } +} + +void EKF2::PublishStates(const hrt_abstime ×tamp) +{ + // publish estimator states + estimator_states_s states; + states.timestamp_sample = timestamp; + states.n_states = Ekf::_k_num_states; + _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states); + _ekf.covariances_diagonal().copyTo(states.covariances); + states.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_states_pub.publish(states); +} + +void EKF2::PublishStatus(const hrt_abstime ×tamp) +{ + estimator_status_s status{}; + status.timestamp_sample = timestamp; + + _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); + + _ekf.get_gps_check_status(&status.gps_check_fail_flags); + + // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include + // the GPS Fix bit, which is always checked) + status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1; + + status.control_mode_flags = _ekf.control_status().value; + status.filter_fault_flags = _ekf.fault_status().value; + + uint16_t innov_check_flags_temp = 0; + _ekf.get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio, + status.vel_test_ratio, status.pos_test_ratio, + status.hgt_test_ratio, status.tas_test_ratio, + status.hagl_test_ratio, status.beta_test_ratio); + + // Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition + // TODO: legacy use only, those flags are also in estimator_status_flags + status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1); + + _ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy); + _ekf.get_ekf_soln_status(&status.solution_status_flags); + _ekf.getImuVibrationMetrics().copyTo(status.vibe); + + // reset counters + status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter; + status.reset_count_vel_d = _ekf.state_reset_status().velD_counter; + status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter; + status.reset_count_pod_d = _ekf.state_reset_status().posD_counter; + status.reset_count_quat = _ekf.state_reset_status().quat_counter; + + status.time_slip = _last_time_slip_us * 1e-6f; + + status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed(); + status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); + status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); + status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); + status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; + + status.accel_device_id = _device_id_accel; + status.baro_device_id = _device_id_baro; + status.gyro_device_id = _device_id_gyro; + status.mag_device_id = _device_id_mag; + + status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_status_pub.publish(status); +} + +void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) +{ + // publish at ~ 1 Hz (or immediately if filter control status or fault status changes) + bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s); + + // filter control status + if (_ekf.control_status().value != _filter_control_status) { + update = true; + _filter_control_status = _ekf.control_status().value; + _filter_control_status_changes++; + } + + // filter fault status + if (_ekf.fault_status().value != _filter_fault_status) { + update = true; + _filter_fault_status = _ekf.fault_status().value; + _filter_fault_status_changes++; + } + + // innovation check fail status + if (_ekf.innov_check_fail_status().value != _innov_check_fail_status) { + update = true; + _innov_check_fail_status = _ekf.innov_check_fail_status().value; + _innov_check_fail_status_changes++; + } + + if (update) { + estimator_status_flags_s status_flags{}; + status_flags.timestamp_sample = timestamp; + + status_flags.control_status_changes = _filter_control_status_changes; + status_flags.cs_tilt_align = _ekf.control_status_flags().tilt_align; + status_flags.cs_yaw_align = _ekf.control_status_flags().yaw_align; + status_flags.cs_gps = _ekf.control_status_flags().gps; + status_flags.cs_opt_flow = _ekf.control_status_flags().opt_flow; + status_flags.cs_mag_hdg = _ekf.control_status_flags().mag_hdg; + status_flags.cs_mag_3d = _ekf.control_status_flags().mag_3D; + status_flags.cs_mag_dec = _ekf.control_status_flags().mag_dec; + status_flags.cs_in_air = _ekf.control_status_flags().in_air; + status_flags.cs_wind = _ekf.control_status_flags().wind; + status_flags.cs_baro_hgt = _ekf.control_status_flags().baro_hgt; + status_flags.cs_rng_hgt = _ekf.control_status_flags().rng_hgt; + status_flags.cs_gps_hgt = _ekf.control_status_flags().gps_hgt; + status_flags.cs_ev_pos = _ekf.control_status_flags().ev_pos; + status_flags.cs_ev_yaw = _ekf.control_status_flags().ev_yaw; + status_flags.cs_ev_hgt = _ekf.control_status_flags().ev_hgt; + status_flags.cs_fuse_beta = _ekf.control_status_flags().fuse_beta; + status_flags.cs_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; + status_flags.cs_fixed_wing = _ekf.control_status_flags().fixed_wing; + status_flags.cs_mag_fault = _ekf.control_status_flags().mag_fault; + status_flags.cs_fuse_aspd = _ekf.control_status_flags().fuse_aspd; + status_flags.cs_gnd_effect = _ekf.control_status_flags().gnd_effect; + status_flags.cs_rng_stuck = _ekf.control_status_flags().rng_stuck; + status_flags.cs_gps_yaw = _ekf.control_status_flags().gps_yaw; + status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight; + status_flags.cs_ev_vel = _ekf.control_status_flags().ev_vel; + status_flags.cs_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z; + status_flags.cs_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest; + + status_flags.fault_status_changes = _filter_fault_status_changes; + status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; + status_flags.fs_bad_mag_y = _ekf.fault_status_flags().bad_mag_y; + status_flags.fs_bad_mag_z = _ekf.fault_status_flags().bad_mag_z; + status_flags.fs_bad_hdg = _ekf.fault_status_flags().bad_hdg; + status_flags.fs_bad_mag_decl = _ekf.fault_status_flags().bad_mag_decl; + status_flags.fs_bad_airspeed = _ekf.fault_status_flags().bad_airspeed; + status_flags.fs_bad_sideslip = _ekf.fault_status_flags().bad_sideslip; + status_flags.fs_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X; + status_flags.fs_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y; + status_flags.fs_bad_vel_n = _ekf.fault_status_flags().bad_vel_N; + status_flags.fs_bad_vel_e = _ekf.fault_status_flags().bad_vel_E; + status_flags.fs_bad_vel_d = _ekf.fault_status_flags().bad_vel_D; + status_flags.fs_bad_pos_n = _ekf.fault_status_flags().bad_pos_N; + status_flags.fs_bad_pos_e = _ekf.fault_status_flags().bad_pos_E; + status_flags.fs_bad_pos_d = _ekf.fault_status_flags().bad_pos_D; + status_flags.fs_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias; + status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical; + status_flags.fs_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping; + + status_flags.innovation_fault_status_changes = _innov_check_fail_status_changes; + status_flags.reject_hor_vel = _ekf.innov_check_fail_status_flags().reject_hor_vel; + status_flags.reject_ver_vel = _ekf.innov_check_fail_status_flags().reject_ver_vel; + status_flags.reject_hor_pos = _ekf.innov_check_fail_status_flags().reject_hor_pos; + status_flags.reject_ver_pos = _ekf.innov_check_fail_status_flags().reject_ver_pos; + status_flags.reject_mag_x = _ekf.innov_check_fail_status_flags().reject_mag_x; + status_flags.reject_mag_y = _ekf.innov_check_fail_status_flags().reject_mag_y; + status_flags.reject_mag_z = _ekf.innov_check_fail_status_flags().reject_mag_z; + status_flags.reject_yaw = _ekf.innov_check_fail_status_flags().reject_yaw; + status_flags.reject_airspeed = _ekf.innov_check_fail_status_flags().reject_airspeed; + status_flags.reject_sideslip = _ekf.innov_check_fail_status_flags().reject_sideslip; + status_flags.reject_hagl = _ekf.innov_check_fail_status_flags().reject_hagl; + status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X; + status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y; + + status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_status_flags_pub.publish(status_flags); + + _last_status_flag_update = status_flags.timestamp; + } +} + +void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) +{ + static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF, + "yaw_estimator_status_s::yaw wrong size"); + + yaw_estimator_status_s yaw_est_test_data; + + if (_ekf.getDataEKFGSF(&yaw_est_test_data.yaw_composite, &yaw_est_test_data.yaw_variance, + yaw_est_test_data.yaw, + yaw_est_test_data.innov_vn, yaw_est_test_data.innov_ve, + yaw_est_test_data.weight)) { + + yaw_est_test_data.timestamp_sample = timestamp; + yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + + _yaw_est_pub.publish(yaw_est_test_data); + } +} + +void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) +{ + if (_ekf.get_wind_status()) { + // Publish wind estimate only if ekf declares them valid + wind_s wind{}; + wind.timestamp_sample = timestamp; + + const Vector2f wind_vel = _ekf.getWindVelocity(); + const Vector2f wind_vel_var = _ekf.getWindVelocityVariance(); + _ekf.getAirspeedInnov(wind.tas_innov); + _ekf.getAirspeedInnovVar(wind.tas_innov_var); + _ekf.getBetaInnov(wind.beta_innov); + _ekf.getBetaInnovVar(wind.beta_innov_var); + + wind.windspeed_north = wind_vel(0); + wind.windspeed_east = wind_vel(1); + wind.variance_north = wind_vel_var(0); + wind.variance_east = wind_vel_var(1); + wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + + _wind_pub.publish(wind); + } +} + +void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp, const optical_flow_s &flow_sample) +{ + estimator_optical_flow_vel_s flow_vel{}; + flow_vel.timestamp_sample = flow_sample.timestamp; + + _ekf.getFlowVelBody().copyTo(flow_vel.vel_body); + _ekf.getFlowVelNE().copyTo(flow_vel.vel_ne); + _ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral); + _ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral); + _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral); + flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + + _estimator_optical_flow_vel_pub.publish(flow_vel); +} + +float EKF2::filter_altitude_ellipsoid(float amsl_hgt) +{ + float height_diff = static_cast(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt; + + if (_gps_alttitude_ellipsoid_previous_timestamp == 0) { + + _wgs84_hgt_offset = height_diff; + _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec; + + } else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) { + + // apply a 10 second first order low pass filter to baro offset + float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp); + _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec; + float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset); + _wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f); + } + + return amsl_hgt + _wgs84_hgt_offset; +} + +void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) +{ + // EKF airspeed sample + const unsigned last_generation = _airspeed_sub.get_last_generation(); + airspeed_s airspeed; + + if (_airspeed_sub.update(&airspeed)) { + if (_msg_missed_airspeed_perf == nullptr) { + _msg_missed_airspeed_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed messages missed"); + + } else if (_airspeed_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_airspeed_perf); + } + + // The airspeed measurement received via the airspeed.msg topic has not been corrected + // for scale favtor errors and requires the ASPD_SCALE correction to be applied. + // This could be avoided if true_airspeed_m_s from the airspeed-validated.msg topic + // was used instead, however this would introduce a potential circular dependency + // via the wind estimator that uses EKF velocity estimates. + const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor; + + // only set airspeed data if condition for airspeed fusion are met + if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (true_airspeed_m_s > _param_ekf2_arsp_thr.get())) { + + airspeedSample airspeed_sample { + .time_us = airspeed.timestamp, + .true_airspeed = true_airspeed_m_s, + .eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s, + }; + _ekf.setAirspeedData(airspeed_sample); + } + + ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + } +} + +void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) +{ + // EKF auxillary velocity sample + // - use the landing target pose estimate as another source of velocity data + const unsigned last_generation = _landing_target_pose_sub.get_last_generation(); + landing_target_pose_s landing_target_pose; + + if (_landing_target_pose_sub.update(&landing_target_pose)) { + if (_msg_missed_landing_target_pose_perf == nullptr) { + _msg_missed_landing_target_pose_perf = perf_alloc(PC_COUNT, MODULE_NAME": landing_target_pose messages missed"); + + } else if (_landing_target_pose_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_landing_target_pose_perf); + } + + // we can only use the landing target if it has a fixed position and a valid velocity estimate + if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) { + // velocity of vehicle relative to target has opposite sign to target relative to vehicle + auxVelSample auxvel_sample{ + .time_us = landing_target_pose.timestamp, + .vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f}, + .velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f}, + }; + _ekf.setAuxVelData(auxvel_sample); + } + } +} + +void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) +{ + // EKF baro sample + const unsigned last_generation = _airdata_sub.get_last_generation(); + vehicle_air_data_s airdata; + + if (_airdata_sub.update(&airdata)) { + if (_msg_missed_air_data_perf == nullptr) { + _msg_missed_air_data_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_air_data messages missed"); + + } else if (_airdata_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_air_data_perf); + } + + _ekf.set_air_density(airdata.rho); + + _ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter}); + + _device_id_baro = airdata.baro_device_id; + + ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + } +} + +bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom) +{ + // EKF external vision sample + bool new_ev_odom = false; + const unsigned last_generation = _ev_odom_sub.get_last_generation(); + + if (_ev_odom_sub.update(&ev_odom)) { + if (_msg_missed_odometry_perf == nullptr) { + _msg_missed_odometry_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_visual_odometry messages missed"); + + } else if (_ev_odom_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_odometry_perf); + } + + extVisionSample ev_data{}; + + // if error estimates are unavailable, use parameter defined defaults + + // check for valid velocity data + if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) { + ev_data.vel(0) = ev_odom.vx; + ev_data.vel(1) = ev_odom.vy; + ev_data.vel(2) = ev_odom.vz; + + if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) { + ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD; + + } else { + ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD; + } + + // velocity measurement error from ev_data or parameters + float param_evv_noise_var = sq(_param_ekf2_evv_noise.get()); + + if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]) + && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]) + && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) { + ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]; + ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1]; + ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2]; + ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]; + ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7]; + ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE]; + + } else { + ev_data.velCov = matrix::eye() * param_evv_noise_var; + } + } + + // check for valid position data + if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) { + ev_data.pos(0) = ev_odom.x; + ev_data.pos(1) = ev_odom.y; + ev_data.pos(2) = ev_odom.z; + + float param_evp_noise_var = sq(_param_ekf2_evp_noise.get()); + + // position measurement error from ev_data or parameters + if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]) + && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]) + && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) { + ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]); + ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]); + ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]); + + } else { + ev_data.posVar.setAll(param_evp_noise_var); + } + } + + // check for valid orientation data + if (PX4_ISFINITE(ev_odom.q[0])) { + ev_data.quat = Quatf(ev_odom.q); + + // orientation measurement error from ev_data or parameters + float param_eva_noise_var = sq(_param_ekf2_eva_noise.get()); + + if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) { + ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]); + + } else { + ev_data.angVar = param_eva_noise_var; + } + } + + // use timestamp from external computer, clocks are synchronized when using MAVROS + ev_data.time_us = ev_odom.timestamp_sample; + _ekf.setExtVisionData(ev_data); + + new_ev_odom = true; + + ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + } + + return new_ev_odom; +} + +bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow) +{ + // EKF flow sample + bool new_optical_flow = false; + const unsigned last_generation = _optical_flow_sub.get_last_generation(); + + if (_optical_flow_sub.update(&optical_flow)) { + if (_msg_missed_optical_flow_perf == nullptr) { + _msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed"); + + } else if (_optical_flow_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_optical_flow_perf); + } + + flowSample flow { + .time_us = optical_flow.timestamp, + // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate + // is produced by a RH rotation of the image about the sensor axis. + .flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral}, + .gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral}, + .dt = 1e-6f * (float)optical_flow.integration_timespan, + .quality = optical_flow.quality, + }; + + if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && + PX4_ISFINITE(optical_flow.pixel_flow_x_integral) && + flow.dt < 1) { + + // Save sensor limits reported by the optical flow sensor + _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, + optical_flow.max_ground_distance); + + _ekf.setOpticalFlowData(flow); + + new_optical_flow = true; + } + + ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + } + + return new_optical_flow; +} + +void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) +{ + // EKF GPS message + const unsigned last_generation = _vehicle_gps_position_sub.get_last_generation(); + vehicle_gps_position_s vehicle_gps_position; + + if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) { + if (_msg_missed_gps_perf == nullptr) { + _msg_missed_gps_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_gps_position messages missed"); + + } else if (_vehicle_gps_position_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_gps_perf); + } + + gps_message gps_msg{ + .time_usec = vehicle_gps_position.timestamp, + .lat = vehicle_gps_position.lat, + .lon = vehicle_gps_position.lon, + .alt = vehicle_gps_position.alt, + .yaw = vehicle_gps_position.heading, + .yaw_offset = vehicle_gps_position.heading_offset, + .fix_type = vehicle_gps_position.fix_type, + .eph = vehicle_gps_position.eph, + .epv = vehicle_gps_position.epv, + .sacc = vehicle_gps_position.s_variance_m_s, + .vel_m_s = vehicle_gps_position.vel_m_s, + .vel_ned = Vector3f{ + vehicle_gps_position.vel_n_m_s, + vehicle_gps_position.vel_e_m_s, + vehicle_gps_position.vel_d_m_s + }, + .vel_ned_valid = vehicle_gps_position.vel_ned_valid, + .nsats = vehicle_gps_position.satellites_used, + .pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop + + vehicle_gps_position.vdop * vehicle_gps_position.vdop), + }; + _ekf.setGpsData(gps_msg); + + _gps_time_usec = gps_msg.time_usec; + _gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid; + } +} + +void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) +{ + const unsigned last_generation = _magnetometer_sub.get_last_generation(); + vehicle_magnetometer_s magnetometer; + + if (_magnetometer_sub.update(&magnetometer)) { + if (_msg_missed_magnetometer_perf == nullptr) { + _msg_missed_magnetometer_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_magnetometer messages missed"); + + } else if (_magnetometer_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_magnetometer_perf); + } + + bool reset = false; + + // check if magnetometer has changed + if (magnetometer.device_id != _device_id_mag) { + if (_device_id_mag != 0) { + PX4_WARN("%d - mag sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_mag, magnetometer.device_id); + } + + reset = true; + + } else if (magnetometer.calibration_count > _mag_calibration_count) { + // existing calibration has changed, reset saved mag bias + PX4_DEBUG("%d - mag %" PRIu32 " calibration updated, resetting bias", _instance, _device_id_mag); + reset = true; + } + + if (reset) { + _ekf.resetMagBias(); + _device_id_mag = magnetometer.device_id; + _mag_calibration_count = magnetometer.calibration_count; + + // reset magnetometer bias learning + _mag_cal_total_time_us = 0; + _mag_cal_last_us = 0; + _mag_cal_available = false; + } + + _ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}}); + + ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + } +} + +void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) +{ + if (!_distance_sensor_selected) { + // get subscription index of first downward-facing range sensor + uORB::SubscriptionMultiArray distance_sensor_subs{ORB_ID::distance_sensor}; + + if (distance_sensor_subs.advertised()) { + for (unsigned i = 0; i < distance_sensor_subs.size(); i++) { + distance_sensor_s distance_sensor; + + if (distance_sensor_subs[i].copy(&distance_sensor)) { + // only use the first instace which has the correct orientation + if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms) + && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { + + if (_distance_sensor_sub.ChangeInstance(i)) { + PX4_INFO("%d - selected distance_sensor:%d", _instance, i); + _distance_sensor_selected = true; + break; + } + } + } + } + } + } + + // EKF range sample + const unsigned last_generation = _distance_sensor_sub.get_last_generation(); + distance_sensor_s distance_sensor; + + if (_distance_sensor_sub.update(&distance_sensor)) { + if (_msg_missed_distance_sensor_perf == nullptr) { + _msg_missed_distance_sensor_perf = perf_alloc(PC_COUNT, MODULE_NAME": distance_sensor messages missed"); + + } else if (_distance_sensor_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_distance_sensor_perf); + } + + if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { + rangeSample range_sample { + .time_us = distance_sensor.timestamp, + .rng = distance_sensor.current_distance, + .quality = distance_sensor.signal_quality, + }; + _ekf.setRangeData(range_sample); + + // Save sensor limits reported by the rangefinder + _ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance); + + _last_range_sensor_update = distance_sensor.timestamp; + return; + } + + ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + } + + if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) { + _distance_sensor_selected = false; + } +} + +void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) +{ + // Check if conditions are OK for learning of magnetometer bias values + // the EKF is operating in the correct mode and there are no filter faults + if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) { + + if (_mag_cal_last_us != 0) { + _mag_cal_total_time_us += timestamp - _mag_cal_last_us; + + // Start checking mag bias estimates when we have accumulated sufficient calibration time + if (_mag_cal_total_time_us > 30_s) { + _mag_cal_last_bias = _ekf.getMagBias(); + _mag_cal_last_bias_variance = _ekf.getMagBiasVariance(); + _mag_cal_available = true; + } + } + + _mag_cal_last_us = timestamp; + + } else { + // conditions are NOT OK for learning magnetometer bias, reset timestamp + // but keep the accumulated calibration time + _mag_cal_last_us = 0; + + if (_ekf.fault_status().value != 0) { + // if a filter fault has occurred, assume previous learning was invalid and do not + // count it towards total learning time. + _mag_cal_total_time_us = 0; + } + } + + if (!_armed) { + // update stored declination value + if (!_mag_decl_saved) { + float declination_deg; + + if (_ekf.get_mag_decl_deg(&declination_deg)) { + _param_ekf2_mag_decl.set(declination_deg); + _mag_decl_saved = true; + + if (!_multi_mode) { + _param_ekf2_mag_decl.commit_no_notification(); + } + } + } + } +} + +int EKF2::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int EKF2::task_spawn(int argc, char *argv[]) +{ + bool success = false; + bool replay_mode = false; + + if (argc > 1 && !strcmp(argv[1], "-r")) { + PX4_INFO("replay mode enabled"); + replay_mode = true; + } + +#if !defined(CONSTRAINED_FLASH) + bool multi_mode = false; + int32_t imu_instances = 0; + int32_t mag_instances = 0; + + int32_t sens_imu_mode = 1; + param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode); + + if (sens_imu_mode == 0) { + // ekf selector requires SENS_IMU_MODE = 0 + multi_mode = true; + + // IMUs (1 - 4 supported) + param_get(param_find("EKF2_MULTI_IMU"), &imu_instances); + + if (imu_instances < 1 || imu_instances > 4) { + const int32_t imu_instances_limited = math::constrain(imu_instances, static_cast(1), static_cast(4)); + PX4_WARN("EKF2_MULTI_IMU limited %" PRId32 " -> %" PRId32, imu_instances, imu_instances_limited); + param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited); + imu_instances = imu_instances_limited; + } + + int32_t sens_mag_mode = 1; + param_get(param_find("SENS_MAG_MODE"), &sens_mag_mode); + + if (sens_mag_mode == 0) { + param_get(param_find("EKF2_MULTI_MAG"), &mag_instances); + + // Mags (1 - 4 supported) + if (mag_instances < 1 || mag_instances > 4) { + const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast(1), static_cast(4)); + PX4_WARN("EKF2_MULTI_MAG limited %" PRId32 " -> %" PRId32, mag_instances, mag_instances_limited); + param_set_no_notification(param_find("EKF2_MULTI_MAG"), &mag_instances_limited); + mag_instances = mag_instances_limited; + } + + } else { + mag_instances = 1; + } + } + + if (multi_mode) { + // Start EKF2Selector if it's not already running + if (_ekf2_selector.load() == nullptr) { + EKF2Selector *inst = new EKF2Selector(); + + if (inst) { + _ekf2_selector.store(inst); + + } else { + PX4_ERR("Failed to create EKF2 selector"); + return PX4_ERROR; + } + } + + const hrt_abstime time_started = hrt_absolute_time(); + const int multi_instances = math::min(imu_instances * mag_instances, static_cast(EKF2_MAX_INSTANCES)); + int multi_instances_allocated = 0; + + // allocate EKF2 instances until all found or arming + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + + bool ekf2_instance_created[4][4] {}; // IMUs * mags + + while ((multi_instances_allocated < multi_instances) + && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) + && ((hrt_elapsed_time(&time_started) < 30_s) + || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) { + + vehicle_status_sub.update(); + + for (uint8_t mag = 0; mag < mag_instances; mag++) { + uORB::SubscriptionData vehicle_mag_sub{ORB_ID(vehicle_magnetometer), mag}; + + for (uint8_t imu = 0; imu < imu_instances; imu++) { + + uORB::SubscriptionData vehicle_imu_sub{ORB_ID(vehicle_imu), imu}; + vehicle_mag_sub.update(); + + // Mag & IMU data must be valid, first mag can be ignored initially + if ((vehicle_mag_sub.get().device_id != 0 || mag == 0) + && (vehicle_imu_sub.get().accel_device_id != 0) + && (vehicle_imu_sub.get().gyro_device_id != 0)) { + + if (!ekf2_instance_created[imu][mag]) { + EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false); + + if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) { + int actual_instance = ekf2_inst->instance(); // match uORB instance numbering + + if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) { + _objects[actual_instance].store(ekf2_inst); + success = true; + multi_instances_allocated++; + ekf2_instance_created[imu][mag] = true; + + if (actual_instance == 0) { + // force selector to run immediately if first instance started + _ekf2_selector.load()->ScheduleNow(); + } + + PX4_INFO("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance, + imu, vehicle_imu_sub.get().accel_device_id, + mag, vehicle_mag_sub.get().device_id); + + // sleep briefly before starting more instances + px4_usleep(10000); + + } else { + PX4_ERR("instance numbering problem instance: %d", actual_instance); + delete ekf2_inst; + break; + } + + } else { + PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag); + px4_usleep(1000000); + break; + } + } + + } else { + px4_usleep(50000); // give the sensors extra time to start + continue; + } + } + } + + if (multi_instances_allocated < multi_instances) { + px4_usleep(100000); + } + } + + } + +#endif // !CONSTRAINED_FLASH + + else { + // otherwise launch regular + EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode); + + if (ekf2_inst) { + _objects[0].store(ekf2_inst); + ekf2_inst->ScheduleNow(); + success = true; + } + } + + return success ? PX4_OK : PX4_ERROR; +} + +int EKF2::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Attitude and position estimator using an Extended Kalman Filter. It is used for Multirotors and Fixed-Wing. + +The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html) page. + +ekf2 can be started in replay mode (`-r`): in this mode it does not access the system time, but only uses the +timestamps from the sensor topics. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ekf2", "estimator"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +#if !defined(CONSTRAINED_FLASH) + PRINT_MODULE_USAGE_COMMAND_DESCR("select_instance", "Request switch to new estimator instance"); + PRINT_MODULE_USAGE_ARG("", "Specify desired estimator instance", false); +#endif // !CONSTRAINED_FLASH + return 0; +} + +extern "C" __EXPORT int ekf2_main(int argc, char *argv[]) +{ + if (argc <= 1 || strcmp(argv[1], "-h") == 0) { + return EKF2::print_usage(); + } + + if (strcmp(argv[1], "start") == 0) { + int ret = 0; + EKF2::lock_module(); + + ret = EKF2::task_spawn(argc - 1, argv + 1); + + if (ret < 0) { + PX4_ERR("start failed (%i)", ret); + } + + EKF2::unlock_module(); + return ret; + +#if !defined(CONSTRAINED_FLASH) + } else if (strcmp(argv[1], "select_instance") == 0) { + + if (EKF2::trylock_module()) { + if (_ekf2_selector.load()) { + if (argc > 2) { + int instance = atoi(argv[2]); + _ekf2_selector.load()->RequestInstance(instance); + } else { + EKF2::unlock_module(); + return EKF2::print_usage("instance required"); + } + + } else { + PX4_ERR("multi-EKF not active, unable to select instance"); + } + + EKF2::unlock_module(); + + } else { + PX4_WARN("module locked, try again later"); + } + + return 0; +#endif // !CONSTRAINED_FLASH + } else if (strcmp(argv[1], "status") == 0) { + if (EKF2::trylock_module()) { +#if !defined(CONSTRAINED_FLASH) + if (_ekf2_selector.load()) { + _ekf2_selector.load()->PrintStatus(); + } +#endif // !CONSTRAINED_FLASH + + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + if (_objects[i].load()) { + PX4_INFO_RAW("\n"); + _objects[i].load()->print_status(); + } + } + + EKF2::unlock_module(); + + } else { + PX4_WARN("module locked, try again later"); + } + + return 0; + + } else if (strcmp(argv[1], "stop") == 0) { + EKF2::lock_module(); + + if (argc > 2) { + int instance = atoi(argv[2]); + + if (instance >= 0 && instance < EKF2_MAX_INSTANCES) { + PX4_INFO("stopping instance %d", instance); + EKF2 *inst = _objects[instance].load(); + + if (inst) { + inst->request_stop(); + px4_usleep(20000); // 20 ms + delete inst; + _objects[instance].store(nullptr); + } + } else { + PX4_ERR("invalid instance %d", instance); + } + + } else { + // otherwise stop everything + bool was_running = false; + +#if !defined(CONSTRAINED_FLASH) + if (_ekf2_selector.load()) { + PX4_INFO("stopping ekf2 selector"); + _ekf2_selector.load()->Stop(); + delete _ekf2_selector.load(); + _ekf2_selector.store(nullptr); + was_running = true; + } +#endif // !CONSTRAINED_FLASH + + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + EKF2 *inst = _objects[i].load(); + + if (inst) { + PX4_INFO("stopping ekf2 instance %d", i); + was_running = true; + inst->request_stop(); + px4_usleep(20000); // 20 ms + delete inst; + _objects[i].store(nullptr); + } + } + + if (!was_running) { + PX4_WARN("not running"); + } + } + + EKF2::unlock_module(); + return PX4_OK; + } + + EKF2::lock_module(); // Lock here, as the method could access _object. + int ret = EKF2::custom_command(argc - 1, argv + 1); + EKF2::unlock_module(); + + return ret; +} diff --git a/src/prometheus_px4/src/modules/ekf2/EKF2.hpp b/src/prometheus_px4/src/modules/ekf2/EKF2.hpp new file mode 100644 index 0000000..81364d8 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/EKF2.hpp @@ -0,0 +1,522 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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 EKF2.cpp + * Implementation of the attitude and position estimator. + * + * @author Roman Bapst + */ + +#pragma once + +#include "EKF2Selector.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Utility/PreFlightChecker.hpp" + +extern pthread_mutex_t ekf2_module_mutex; + +class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + EKF2() = delete; + EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode); + ~EKF2() 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); + + int print_status(); + + bool should_exit() const { return _task_should_exit.load(); } + + void request_stop() { _task_should_exit.store(true); } + + static void lock_module() { pthread_mutex_lock(&ekf2_module_mutex); } + static bool trylock_module() { return (pthread_mutex_trylock(&ekf2_module_mutex) == 0); } + static void unlock_module() { pthread_mutex_unlock(&ekf2_module_mutex); } + + bool multi_init(int imu, int mag); + + int instance() const { return _instance; } + +private: + void Run() override; + + void PublishAttitude(const hrt_abstime ×tamp); + void PublishEkfDriftMetrics(const hrt_abstime ×tamp); + void PublishEventFlags(const hrt_abstime ×tamp); + void PublishGlobalPosition(const hrt_abstime ×tamp); + void PublishInnovations(const hrt_abstime ×tamp, const imuSample &imu); + void PublishInnovationTestRatios(const hrt_abstime ×tamp); + void PublishInnovationVariances(const hrt_abstime ×tamp); + void PublishLocalPosition(const hrt_abstime ×tamp); + void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu); + void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom); + void PublishOpticalFlowVel(const hrt_abstime ×tamp, const optical_flow_s &optical_flow); + void PublishSensorBias(const hrt_abstime ×tamp); + void PublishStates(const hrt_abstime ×tamp); + void PublishStatus(const hrt_abstime ×tamp); + void PublishStatusFlags(const hrt_abstime ×tamp); + void PublishWindEstimate(const hrt_abstime ×tamp); + void PublishYawEstimatorStatus(const hrt_abstime ×tamp); + + void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); + bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom); + bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow); + void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); + + void UpdateMagCalibration(const hrt_abstime ×tamp); + + /* + * Calculate filtered WGS84 height from estimated AMSL height + */ + float filter_altitude_ellipsoid(float amsl_hgt); + + static constexpr float sq(float x) { return x * x; }; + + const bool _replay_mode{false}; ///< true when we use replay data from a log + const bool _multi_mode; + int _instance{0}; + + px4::atomic_bool _task_should_exit{false}; + + // time slip monitoring + uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec) + uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) + int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) + + perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; + perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; + perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; + perf_counter_t _msg_missed_air_data_perf{nullptr}; + perf_counter_t _msg_missed_airspeed_perf{nullptr}; + perf_counter_t _msg_missed_distance_sensor_perf{nullptr}; + perf_counter_t _msg_missed_gps_perf{nullptr}; + perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; + perf_counter_t _msg_missed_magnetometer_perf{nullptr}; + perf_counter_t _msg_missed_odometry_perf{nullptr}; + perf_counter_t _msg_missed_optical_flow_perf{nullptr}; + + // Used to check, save and use learned magnetometer biases + hrt_abstime _mag_cal_last_us{0}; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec) + hrt_abstime _mag_cal_total_time_us{0}; ///< accumulated calibration time since the last save + + Vector3f _mag_cal_last_bias{}; ///< last valid XYZ magnetometer bias estimates (Gauss) + Vector3f _mag_cal_last_bias_variance{}; ///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2) + bool _mag_cal_available{false}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available + + // Used to control saving of mag declination to be used on next startup + bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved + + bool _had_valid_terrain{false}; ///< true if at any time there was a valid terrain estimate + + uint64_t _gps_time_usec{0}; + int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid + uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt + float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 + + uint8_t _imu_calibration_count{0}; + uint8_t _mag_calibration_count{0}; + + uint32_t _device_id_accel{0}; + uint32_t _device_id_baro{0}; + uint32_t _device_id_gyro{0}; + uint32_t _device_id_mag{0}; + + Vector3f _last_accel_bias_published{}; + Vector3f _last_gyro_bias_published{}; + Vector3f _last_mag_bias_published{}; + + float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)}; + uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; + uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; + uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; + uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; + uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + + uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; + uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; + + bool _callback_registered{false}; + + bool _distance_sensor_selected{false}; // because we can have several distance sensor instances with different orientations + bool _armed{false}; + bool _standby{false}; // standby arming state + + hrt_abstime _last_status_flag_update{0}; + hrt_abstime _last_range_sensor_update{0}; + + uint32_t _filter_control_status{0}; + uint32_t _filter_fault_status{0}; + uint32_t _innov_check_fail_status{0}; + + uint32_t _filter_control_status_changes{0}; + uint32_t _filter_fault_status_changes{0}; + uint32_t _innov_check_fail_status_changes{0}; + uint32_t _filter_warning_event_changes{0}; + uint32_t _filter_information_event_changes{0}; + + uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; + uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; + uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; + uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; + uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; + uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; + uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; + uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; + uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; + uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; + uORB::PublicationMulti _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; + uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; + uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; + + // publications with topic dependent on multi-mode + uORB::PublicationMulti _attitude_pub; + uORB::PublicationMulti _local_position_pub; + uORB::PublicationMulti _global_position_pub; + uORB::PublicationMulti _odometry_pub; + uORB::PublicationMulti _wind_pub; + + + PreFlightChecker _preflt_checker; + + Ekf _ekf; + + parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) + + DEFINE_PARAMETERS( + (ParamExtInt) + _param_ekf2_min_obs_dt, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec) + (ParamExtFloat) + _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval + (ParamExtFloat) + _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_avel_delay, ///< auxillary velocity measurement delay relative to the IMU (mSec) + + (ParamExtFloat) + _param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec) + (ParamExtFloat) + _param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2) + + // process noise + (ParamExtFloat) + _param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2) + (ParamExtFloat) + _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3) + (ParamExtFloat) + _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec) + (ParamExtFloat) + _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) + (ParamExtFloat) + _param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2) + (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) + (ParamExtFloat) + _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) + + (ParamExtFloat) + _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) + (ParamExtFloat) + _param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m) + (ParamExtFloat) + _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) + (ParamExtFloat) + _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) + (ParamExtFloat) + _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) + (ParamExtFloat) + _param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m) + (ParamExtFloat) + _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) + + // control of magnetometer fusion + (ParamExtFloat) + _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) + (ParamExtFloat) + _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) + (ParamExtFloat) + _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec) + (ParamExtFloat) + _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) + (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) + (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) + (ParamExtFloat) + _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) + (ParamExtInt) + _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data + (ParamExtInt) + _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used + (ParamExtFloat) + _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used + (ParamExtFloat) + _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) + + (ParamExtInt) + _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used + (ParamExtFloat) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) + (ParamExtFloat) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m) + (ParamExtFloat) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) + (ParamExtInt) _param_ekf2_req_nsats, ///< minimum acceptable satellite count + (ParamExtFloat) + _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision + (ParamExtFloat) + _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) + (ParamExtFloat) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) + + // measurement source control + (ParamExtInt) + _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used + (ParamExtInt) _param_ekf2_hgt_mode, ///< selects the primary source for height data + (ParamExtInt) + _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation + (ParamExtInt) + _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) + + // range finder fusion + (ParamExtFloat) + _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) + (ParamExtFloat) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) + (ParamExtFloat) + _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) + (ParamExtFloat) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) + (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) + (ParamExtInt) + _param_ekf2_rng_aid, ///< enables use of a range finder even if primary height source is not range finder + (ParamExtFloat) + _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) + (ParamExtFloat) + _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) + (ParamExtFloat) + _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) + (ParamExtFloat) + _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) + + // vision estimate fusion + (ParamInt) + _param_ekf2_ev_noise_md, ///< determine source of vision observation noise + (ParamFloat) + _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) + (ParamFloat) + _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) + (ParamFloat) + _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) + (ParamExtFloat) + _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) + + // optical flow fusion + (ParamExtFloat) + _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) + (ParamExtFloat) + _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) + (ParamExtInt) + _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor + (ParamExtFloat) + _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) + + // sensor positions in body frame + (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) + (ParamExtFloat) + _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) + + // control of airspeed and sideslip fusion + (ParamFloat) + _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) + (ParamInt) + _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables + + // output predictor filter time constants + (ParamExtFloat) + _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec) + (ParamExtFloat) + _param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec) + + // IMU switch on bias parameters + (ParamExtFloat) + _param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) + (ParamExtFloat) + _param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) + (ParamExtFloat) + _param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad) + + // EKF accel bias learning control + (ParamExtFloat) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2) + (ParamExtFloat) + _param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2) + (ParamExtFloat) + _param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2) + (ParamExtFloat) + _param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec) + + // Multi-rotor drag specific force fusion + (ParamExtFloat) + _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 + (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) + (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) + + // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth + // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 + (ParamExtFloat) + _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) + (ParamExtFloat) + _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis + (ParamExtFloat) + _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis + (ParamExtFloat) + _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis + (ParamExtFloat) + _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis + (ParamExtFloat) + _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis + + // Test used to determine if the vehicle is static or moving + (ParamExtFloat) + _param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving. + + (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time + (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check + (ParamExtInt) + _param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone. + + // Used by EKF-GSF experimental yaw estimator + (ParamExtFloat) + _param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation + + ) +}; diff --git a/src/prometheus_px4/src/modules/ekf2/EKF2Selector.cpp b/src/prometheus_px4/src/modules/ekf2/EKF2Selector.cpp new file mode 100644 index 0000000..bacdc4e --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/EKF2Selector.cpp @@ -0,0 +1,827 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "EKF2Selector.hpp" + +using namespace time_literals; +using matrix::Quatf; +using matrix::Vector2f; +using math::constrain; +using math::radians; + +EKF2Selector::EKF2Selector() : + ModuleParams(nullptr), + ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers) +{ +} + +EKF2Selector::~EKF2Selector() +{ + Stop(); +} + +bool EKF2Selector::Start() +{ + ScheduleNow(); + return true; +} + +void EKF2Selector::Stop() +{ + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + _instance[i].estimator_attitude_sub.unregisterCallback(); + _instance[i].estimator_status_sub.unregisterCallback(); + } + + ScheduleClear(); +} + +void EKF2Selector::PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance) +{ + const char *old_reason = nullptr; + + if (_instance[old_instance].filter_fault) { + old_reason = " (filter fault)"; + + } else if (_instance[old_instance].timeout) { + old_reason = " (timeout)"; + + } else if (_gyro_fault_detected) { + old_reason = " (gyro fault)"; + + } else if (_accel_fault_detected) { + old_reason = " (accel fault)"; + + } else if (!_instance[_selected_instance].healthy && (_instance[_selected_instance].healthy_count > 0)) { + // skipped if previous instance was never healthy in the first place (eg initialization) + old_reason = " (unhealthy)"; + } + + const char *new_reason = nullptr; + + if (_request_instance.load() == new_instance) { + new_reason = " (user selected)"; + } + + if (old_reason || new_reason) { + if (old_reason == nullptr) { + old_reason = ""; + } + + if (new_reason == nullptr) { + new_reason = ""; + } + + PX4_WARN("primary EKF changed %" PRIu8 "%s -> %" PRIu8 "%s", old_instance, old_reason, new_instance, new_reason); + } +} + +bool EKF2Selector::SelectInstance(uint8_t ekf_instance) +{ + if ((ekf_instance != _selected_instance) && (ekf_instance < _available_instances)) { + // update sensor_selection immediately + sensor_selection_s sensor_selection{}; + sensor_selection.accel_device_id = _instance[ekf_instance].accel_device_id; + sensor_selection.gyro_device_id = _instance[ekf_instance].gyro_device_id; + sensor_selection.timestamp = hrt_absolute_time(); + _sensor_selection_pub.publish(sensor_selection); + + if (_selected_instance != INVALID_INSTANCE) { + // switch callback registration + _instance[_selected_instance].estimator_attitude_sub.unregisterCallback(); + _instance[_selected_instance].estimator_status_sub.unregisterCallback(); + + PrintInstanceChange(_selected_instance, ekf_instance); + } + + _instance[ekf_instance].estimator_attitude_sub.registerCallback(); + _instance[ekf_instance].estimator_status_sub.registerCallback(); + + _selected_instance = ekf_instance; + _instance_changed_count++; + _last_instance_change = sensor_selection.timestamp; + _instance[ekf_instance].time_last_selected = _last_instance_change; + + // reset all relative test ratios + for (uint8_t i = 0; i < _available_instances; i++) { + _instance[i].relative_test_ratio = 0; + } + + return true; + } + + return false; +} + +bool EKF2Selector::UpdateErrorScores() +{ + // first check imu inconsistencies + _gyro_fault_detected = false; + uint32_t faulty_gyro_id = 0; + _accel_fault_detected = false; + uint32_t faulty_accel_id = 0; + + if (_sensors_status_imu.updated()) { + sensors_status_imu_s sensors_status_imu; + + if (_sensors_status_imu.copy(&sensors_status_imu)) { + + const float time_step_s = constrain((sensors_status_imu.timestamp - _last_update_us) * 1e-6f, 0.f, 0.02f); + _last_update_us = sensors_status_imu.timestamp; + + { + const float angle_rate_threshold = radians(_param_ekf2_sel_imu_angle_rate.get()); + const float angle_threshold = radians(_param_ekf2_sel_imu_angle.get()); + uint8_t n_gyros = 0; + uint8_t n_gyro_exceedances = 0; + float largest_accumulated_gyro_error = 0.0f; + uint8_t largest_gyro_error_index = 0; + + for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) { + // check for gyros with excessive difference to mean using accumulated error + if (sensors_status_imu.gyro_device_ids[i] != 0) { + n_gyros++; + _accumulated_gyro_error[i] += (sensors_status_imu.gyro_inconsistency_rad_s[i] - angle_rate_threshold) * time_step_s; + _accumulated_gyro_error[i] = fmaxf(_accumulated_gyro_error[i], 0.f); + + if (_accumulated_gyro_error[i] > angle_threshold) { + n_gyro_exceedances++; + } + + if (_accumulated_gyro_error[i] > largest_accumulated_gyro_error) { + largest_accumulated_gyro_error = _accumulated_gyro_error[i]; + largest_gyro_error_index = i; + } + + } else { + // no sensor + _accumulated_gyro_error[i] = NAN; + } + } + + if (n_gyro_exceedances > 0) { + if (n_gyros >= 3) { + // If there are 3 or more sensors, the one with the largest accumulated error is faulty + _gyro_fault_detected = true; + faulty_gyro_id = sensors_status_imu.gyro_device_ids[largest_gyro_error_index]; + + } else if (n_gyros == 2) { + // A fault is present, but the faulty sensor identity cannot be determined + _gyro_fault_detected = true; + } + } + } + + { + const float accel_threshold = _param_ekf2_sel_imu_accel.get(); + const float velocity_threshold = _param_ekf2_sel_imu_velocity.get(); + uint8_t n_accels = 0; + uint8_t n_accel_exceedances = 0; + float largest_accumulated_accel_error = 0.0f; + uint8_t largest_accel_error_index = 0; + + for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) { + // check for accelerometers with excessive difference to mean using accumulated error + if (sensors_status_imu.accel_device_ids[i] != 0) { + n_accels++; + _accumulated_accel_error[i] += (sensors_status_imu.accel_inconsistency_m_s_s[i] - accel_threshold) * time_step_s; + _accumulated_accel_error[i] = fmaxf(_accumulated_accel_error[i], 0.f); + + if (_accumulated_accel_error[i] > velocity_threshold) { + n_accel_exceedances++; + } + + if (_accumulated_accel_error[i] > largest_accumulated_accel_error) { + largest_accumulated_accel_error = _accumulated_accel_error[i]; + largest_accel_error_index = i; + } + + } else { + // no sensor + _accumulated_accel_error[i] = NAN; + } + } + + if (n_accel_exceedances > 0) { + if (n_accels >= 3) { + // If there are 3 or more sensors, the one with the largest accumulated error is faulty + _accel_fault_detected = true; + faulty_accel_id = sensors_status_imu.accel_device_ids[largest_accel_error_index]; + + } else if (n_accels == 2) { + // A fault is present, but the faulty sensor identity cannot be determined + _accel_fault_detected = true; + } + } + } + } + } + + bool updated = false; + bool primary_updated = false; + + // default estimator timeout + hrt_abstime status_timeout = 50_ms; + + if (hrt_elapsed_time(&_attitude_last.timestamp) > FILTER_UPDATE_PERIOD) { + // much lower timeout if current primary estimator attitude isn't publishing + status_timeout = 2 * FILTER_UPDATE_PERIOD; + } + + // calculate individual error scores + for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { + const bool prev_healthy = _instance[i].healthy; + + estimator_status_s status; + + if (_instance[i].estimator_status_sub.update(&status)) { + + _instance[i].timestamp_sample_last = status.timestamp_sample; + + _instance[i].accel_device_id = status.accel_device_id; + _instance[i].gyro_device_id = status.gyro_device_id; + _instance[i].baro_device_id = status.baro_device_id; + _instance[i].mag_device_id = status.mag_device_id; + + if ((i + 1) > _available_instances) { + _available_instances = i + 1; + updated = true; + } + + if (i == _selected_instance) { + primary_updated = true; + } + + // test ratios are invalid when 0, >= 1 is a failure + if (!PX4_ISFINITE(status.vel_test_ratio) || (status.vel_test_ratio <= 0.f)) { + status.vel_test_ratio = 1.f; + } + + if (!PX4_ISFINITE(status.pos_test_ratio) || (status.pos_test_ratio <= 0.f)) { + status.pos_test_ratio = 1.f; + } + + if (!PX4_ISFINITE(status.hgt_test_ratio) || (status.hgt_test_ratio <= 0.f)) { + status.hgt_test_ratio = 1.f; + } + + float combined_test_ratio = fmaxf(0.5f * (status.vel_test_ratio + status.pos_test_ratio), status.hgt_test_ratio); + + _instance[i].combined_test_ratio = combined_test_ratio; + _instance[i].healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f); + _instance[i].filter_fault = (status.filter_fault_flags != 0); + _instance[i].timeout = false; + + if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) { + _instance[i].relative_test_ratio = 0; + } + + } else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout)) { + _instance[i].healthy = false; + _instance[i].timeout = true; + } + + // if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay + if (_gyro_fault_detected && (faulty_gyro_id != 0) && (_instance[i].gyro_device_id == faulty_gyro_id)) { + _instance[i].healthy = false; + } + + // if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay + if (_accel_fault_detected && (faulty_accel_id != 0) && (_instance[i].accel_device_id == faulty_accel_id)) { + _instance[i].healthy = false; + } + + if (prev_healthy != _instance[i].healthy) { + updated = true; + _selector_status_publish = true; + + if (!prev_healthy) { + _instance[i].healthy_count++; + } + } + } + + // update relative test ratios if primary has updated + if (primary_updated) { + for (uint8_t i = 0; i < _available_instances; i++) { + if (i != _selected_instance) { + + const float error_delta = _instance[i].combined_test_ratio - _instance[_selected_instance].combined_test_ratio; + + // reduce error only if its better than the primary instance by at least EKF2_SEL_ERR_RED to prevent unnecessary selection changes + const float threshold = _gyro_fault_detected ? 0.0f : fmaxf(_param_ekf2_sel_err_red.get(), 0.05f); + + if (error_delta > 0 || error_delta < -threshold) { + _instance[i].relative_test_ratio += error_delta; + _instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim); + + if ((error_delta < -threshold) && (_instance[i].relative_test_ratio < 1.f)) { + // increase status publication rate if there's movement towards a potential instance change + _selector_status_publish = true; + } + } + } + } + } + + return (primary_updated || updated); +} + +void EKF2Selector::PublishVehicleAttitude() +{ + // selected estimator_attitude -> vehicle_attitude + vehicle_attitude_s attitude; + + if (_instance[_selected_instance].estimator_attitude_sub.update(&attitude)) { + bool instance_change = false; + + if (_instance[_selected_instance].estimator_attitude_sub.get_instance() != _attitude_instance_prev) { + _attitude_instance_prev = _instance[_selected_instance].estimator_attitude_sub.get_instance(); + instance_change = true; + } + + if (_attitude_last.timestamp != 0) { + if (!instance_change && (attitude.quat_reset_counter == _attitude_last.quat_reset_counter + 1)) { + // propogate deltas from estimator data while maintaining the overall reset counts + ++_quat_reset_counter; + _delta_q_reset = Quatf{attitude.delta_q_reset}; + + } else if (instance_change || (attitude.quat_reset_counter != _attitude_last.quat_reset_counter)) { + // on reset compute deltas from last published data + ++_quat_reset_counter; + _delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized(); + } + + } else { + _quat_reset_counter = attitude.quat_reset_counter; + _delta_q_reset = Quatf{attitude.delta_q_reset}; + } + + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's attitude for system (vehicle_attitude) if it's stale + if ((attitude.timestamp_sample <= _attitude_last.timestamp_sample) + || (attitude.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary estimator_attitude as published with original resets + _attitude_last = attitude; + + if (publish) { + // republish with total reset count and current timestamp + attitude.quat_reset_counter = _quat_reset_counter; + _delta_q_reset.copyTo(attitude.delta_q_reset); + + attitude.timestamp = hrt_absolute_time(); + _vehicle_attitude_pub.publish(attitude); + } + } +} + +void EKF2Selector::PublishVehicleLocalPosition() +{ + // selected estimator_local_position -> vehicle_local_position + vehicle_local_position_s local_position; + + if (_instance[_selected_instance].estimator_local_position_sub.update(&local_position)) { + bool instance_change = false; + + if (_instance[_selected_instance].estimator_local_position_sub.get_instance() != _local_position_instance_prev) { + _local_position_instance_prev = _instance[_selected_instance].estimator_local_position_sub.get_instance(); + instance_change = true; + } + + if (_local_position_last.timestamp != 0) { + // XY reset + if (!instance_change && (local_position.xy_reset_counter == _local_position_last.xy_reset_counter + 1)) { + ++_xy_reset_counter; + _delta_xy_reset = Vector2f{local_position.delta_xy}; + + } else if (instance_change || (local_position.xy_reset_counter != _local_position_last.xy_reset_counter)) { + ++_xy_reset_counter; + _delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y}; + } + + // Z reset + if (!instance_change && (local_position.z_reset_counter == _local_position_last.z_reset_counter + 1)) { + ++_z_reset_counter; + _delta_z_reset = local_position.delta_z; + + } else if (instance_change || (local_position.z_reset_counter != _local_position_last.z_reset_counter)) { + ++_z_reset_counter; + _delta_z_reset = local_position.z - _local_position_last.z; + } + + // VXY reset + if (!instance_change && (local_position.vxy_reset_counter == _local_position_last.vxy_reset_counter + 1)) { + ++_vxy_reset_counter; + _delta_vxy_reset = Vector2f{local_position.delta_vxy}; + + } else if (instance_change || (local_position.vxy_reset_counter != _local_position_last.vxy_reset_counter)) { + ++_vxy_reset_counter; + _delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy}; + } + + // VZ reset + if (!instance_change && (local_position.vz_reset_counter == _local_position_last.vz_reset_counter + 1)) { + ++_vz_reset_counter; + _delta_vz_reset = local_position.delta_vz; + + } else if (instance_change || (local_position.vz_reset_counter != _local_position_last.vz_reset_counter)) { + ++_vz_reset_counter; + _delta_vz_reset = local_position.vz - _local_position_last.vz; + } + + // heading reset + if (!instance_change && (local_position.heading_reset_counter == _local_position_last.heading_reset_counter + 1)) { + ++_heading_reset_counter; + _delta_heading_reset = local_position.delta_heading; + + } else if (instance_change || (local_position.heading_reset_counter != _local_position_last.heading_reset_counter)) { + ++_heading_reset_counter; + _delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading); + } + + } else { + _xy_reset_counter = local_position.xy_reset_counter; + _z_reset_counter = local_position.z_reset_counter; + _vxy_reset_counter = local_position.vxy_reset_counter; + _vz_reset_counter = local_position.vz_reset_counter; + _heading_reset_counter = local_position.heading_reset_counter; + + _delta_xy_reset = Vector2f{local_position.delta_xy}; + _delta_z_reset = local_position.delta_z; + _delta_vxy_reset = Vector2f{local_position.delta_vxy}; + _delta_vz_reset = local_position.delta_vz; + _delta_heading_reset = local_position.delta_heading; + } + + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's local position for system (vehicle_local_position) if it's stale + if ((local_position.timestamp_sample <= _local_position_last.timestamp_sample) + || (local_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary estimator_local_position as published with original resets + _local_position_last = local_position; + + if (publish) { + // republish with total reset count and current timestamp + local_position.xy_reset_counter = _xy_reset_counter; + local_position.z_reset_counter = _z_reset_counter; + local_position.vxy_reset_counter = _vxy_reset_counter; + local_position.vz_reset_counter = _vz_reset_counter; + local_position.heading_reset_counter = _heading_reset_counter; + + _delta_xy_reset.copyTo(local_position.delta_xy); + local_position.delta_z = _delta_z_reset; + _delta_vxy_reset.copyTo(local_position.delta_vxy); + local_position.delta_vz = _delta_vz_reset; + local_position.delta_heading = _delta_heading_reset; + + local_position.timestamp = hrt_absolute_time(); + _vehicle_local_position_pub.publish(local_position); + } + } +} + +void EKF2Selector::PublishVehicleOdometry() +{ + // selected estimator_odometry -> vehicle_odometry + vehicle_odometry_s odometry; + + if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) { + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's odometry for system (vehicle_odometry) if it's stale + if ((odometry.timestamp_sample <= _odometry_last.timestamp_sample) + || (odometry.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary estimator_odometry + _odometry_last = odometry; + + if (publish) { + odometry.timestamp = hrt_absolute_time(); + _vehicle_odometry_pub.publish(odometry); + } + } +} + +void EKF2Selector::PublishVehicleGlobalPosition() +{ + // selected estimator_global_position -> vehicle_global_position + vehicle_global_position_s global_position; + + if (_instance[_selected_instance].estimator_global_position_sub.update(&global_position)) { + bool instance_change = false; + + if (_instance[_selected_instance].estimator_global_position_sub.get_instance() != _global_position_instance_prev) { + _global_position_instance_prev = _instance[_selected_instance].estimator_global_position_sub.get_instance(); + instance_change = true; + } + + if (_global_position_last.timestamp != 0) { + // lat/lon reset + if (!instance_change && (global_position.lat_lon_reset_counter == _global_position_last.lat_lon_reset_counter + 1)) { + ++_lat_lon_reset_counter; + + // TODO: delta latitude/longitude + _delta_lat_reset = global_position.lat - _global_position_last.lat; + _delta_lon_reset = global_position.lon - _global_position_last.lon; + + } else if (instance_change || (global_position.lat_lon_reset_counter != _global_position_last.lat_lon_reset_counter)) { + ++_lat_lon_reset_counter; + + _delta_lat_reset = global_position.lat - _global_position_last.lat; + _delta_lon_reset = global_position.lon - _global_position_last.lon; + } + + // alt reset + if (!instance_change && (global_position.alt_reset_counter == _global_position_last.alt_reset_counter + 1)) { + ++_alt_reset_counter; + _delta_alt_reset = global_position.delta_alt; + + } else if (instance_change || (global_position.alt_reset_counter != _global_position_last.alt_reset_counter)) { + ++_alt_reset_counter; + _delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt; + } + + } else { + _lat_lon_reset_counter = global_position.lat_lon_reset_counter; + _alt_reset_counter = global_position.alt_reset_counter; + + _delta_alt_reset = global_position.delta_alt; + } + + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's global position for system (vehicle_global_position) if it's stale + if ((global_position.timestamp_sample <= _global_position_last.timestamp_sample) + || (global_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary estimator_global_position as published with original resets + _global_position_last = global_position; + + if (publish) { + // republish with total reset count and current timestamp + global_position.lat_lon_reset_counter = _lat_lon_reset_counter; + global_position.alt_reset_counter = _alt_reset_counter; + global_position.delta_alt = _delta_alt_reset; + + global_position.timestamp = hrt_absolute_time(); + _vehicle_global_position_pub.publish(global_position); + } + } +} + +void EKF2Selector::PublishWindEstimate() +{ + // selected estimator_wind -> wind + wind_s wind; + + if (_instance[_selected_instance].estimator_wind_sub.update(&wind)) { + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's wind for system (wind) if it's stale + if ((wind.timestamp_sample <= _wind_last.timestamp_sample) + || (wind.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary wind + _wind_last = wind; + + // publish estimator's wind for system unless it's stale + if (publish) { + // republish with current timestamp + wind.timestamp = hrt_absolute_time(); + _wind_pub.publish(wind); + } + } +} + +void EKF2Selector::Run() +{ + // re-schedule as backup timeout + ScheduleDelayed(FILTER_UPDATE_PERIOD); + + // 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(); + } + + // update combined test ratio for all estimators + const bool updated = UpdateErrorScores(); + + // if no valid instance then force select first instance with valid IMU + if (_selected_instance == INVALID_INSTANCE) { + for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { + if ((_instance[i].accel_device_id != 0) + && (_instance[i].gyro_device_id != 0)) { + + if (SelectInstance(i)) { + break; + } + } + } + + // if still invalid return early and check again on next scheduled run + if (_selected_instance == INVALID_INSTANCE) { + return; + } + } + + if (updated) { + const uint8_t available_instances_prev = _available_instances; + const uint8_t selected_instance_prev = _selected_instance; + const uint32_t instance_changed_count_prev = _instance_changed_count; + const hrt_abstime last_instance_change_prev = _last_instance_change; + + bool lower_error_available = false; + float alternative_error = 0.f; // looking for instances that have error lower than the current primary + float best_test_ratio = FLT_MAX; + + uint8_t best_ekf = _selected_instance; + uint8_t best_ekf_alternate = INVALID_INSTANCE; + uint8_t best_ekf_different_imu = INVALID_INSTANCE; + + // loop through all available instances to find if an alternative is available + for (int i = 0; i < _available_instances; i++) { + // Use an alternative instance if - + // (healthy and has updated recently) + // AND + // (has relative error less than selected instance and has not been the selected instance for at least 10 seconds + // OR + // selected instance has stopped updating + if (_instance[i].healthy && (i != _selected_instance)) { + const float test_ratio = _instance[i].combined_test_ratio; + const float relative_error = _instance[i].relative_test_ratio; + + if (relative_error < alternative_error) { + best_ekf_alternate = i; + alternative_error = relative_error; + + // relative error less than selected instance and has not been the selected instance for at least 10 seconds + if ((relative_error <= -_rel_err_thresh) && hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s) { + lower_error_available = true; + } + } + + if ((test_ratio > 0) && (test_ratio < best_test_ratio)) { + best_ekf = i; + best_test_ratio = test_ratio; + + // also check next best available ekf using a different IMU + if (_instance[i].accel_device_id != _instance[_selected_instance].accel_device_id) { + best_ekf_different_imu = i; + } + } + } + } + + if (!_instance[_selected_instance].healthy) { + // prefer the best healthy instance using a different IMU + if (!SelectInstance(best_ekf_different_imu)) { + // otherwise switch to the healthy instance with best overall test ratio + SelectInstance(best_ekf); + } + + } else if (lower_error_available && (hrt_elapsed_time(&_last_instance_change) > 10_s)) { + // if this instance has a significantly lower relative error to the active primary, we consider it as a + // better instance and would like to switch to it even if the current primary is healthy + SelectInstance(best_ekf_alternate); + + } else if (_request_instance.load() != INVALID_INSTANCE) { + + const uint8_t new_instance = _request_instance.load(); + + // attempt to switch to user manually selected instance + if (!SelectInstance(new_instance)) { + PX4_ERR("unable to switch to user selected instance %d", new_instance); + } + + // reset + _request_instance.store(INVALID_INSTANCE); + } + + // publish selector status at ~1 Hz or immediately on any change + if (_selector_status_publish || (hrt_elapsed_time(&_last_status_publish) > 1_s) + || (available_instances_prev != _available_instances) + || (selected_instance_prev != _selected_instance) + || (instance_changed_count_prev != _instance_changed_count) + || (last_instance_change_prev != _last_instance_change) + || _accel_fault_detected || _gyro_fault_detected) { + + PublishEstimatorSelectorStatus(); + _selector_status_publish = false; + } + } + + // republish selected estimator data for system + PublishVehicleAttitude(); + PublishVehicleLocalPosition(); + PublishVehicleGlobalPosition(); + PublishVehicleOdometry(); + PublishWindEstimate(); +} + +void EKF2Selector::PublishEstimatorSelectorStatus() +{ + estimator_selector_status_s selector_status{}; + selector_status.primary_instance = _selected_instance; + selector_status.instances_available = _available_instances; + selector_status.instance_changed_count = _instance_changed_count; + selector_status.last_instance_change = _last_instance_change; + selector_status.accel_device_id = _instance[_selected_instance].accel_device_id; + selector_status.baro_device_id = _instance[_selected_instance].baro_device_id; + selector_status.gyro_device_id = _instance[_selected_instance].gyro_device_id; + selector_status.mag_device_id = _instance[_selected_instance].mag_device_id; + selector_status.gyro_fault_detected = _gyro_fault_detected; + selector_status.accel_fault_detected = _accel_fault_detected; + + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio; + selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio; + selector_status.healthy[i] = _instance[i].healthy; + } + + for (int i = 0; i < IMU_STATUS_SIZE; i++) { + selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i]; + selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i]; + } + + selector_status.timestamp = hrt_absolute_time(); + _estimator_selector_status_pub.publish(selector_status); + _last_status_publish = selector_status.timestamp; +} + +void EKF2Selector::PrintStatus() +{ + PX4_INFO("available instances: %" PRIu8, _available_instances); + + if (_selected_instance == INVALID_INSTANCE) { + PX4_WARN("selected instance: None"); + } + + for (int i = 0; i < _available_instances; i++) { + const EstimatorInstance &inst = _instance[i]; + + PX4_INFO("%" PRIu8 ": ACC: %" PRIu32 ", GYRO: %" PRIu32 ", MAG: %" PRIu32 ", %s, test ratio: %.7f (%.5f) %s", + inst.instance, inst.accel_device_id, inst.gyro_device_id, inst.mag_device_id, + inst.healthy ? "healthy" : "unhealthy", + (double)inst.combined_test_ratio, (double)inst.relative_test_ratio, + (_selected_instance == i) ? "*" : ""); + } +} diff --git a/src/prometheus_px4/src/modules/ekf2/EKF2Selector.hpp b/src/prometheus_px4/src/modules/ekf2/EKF2Selector.hpp new file mode 100644 index 0000000..a37f5e5 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/EKF2Selector.hpp @@ -0,0 +1,242 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if CONSTRAINED_MEMORY +# define EKF2_MAX_INSTANCES 2 +#else +# define EKF2_MAX_INSTANCES 9 +#endif + +using namespace time_literals; + +class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + EKF2Selector(); + ~EKF2Selector() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + + void RequestInstance(uint8_t instance) { _request_instance.store(instance); } + +private: + static constexpr uint8_t INVALID_INSTANCE{UINT8_MAX}; + static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms}; + + void Run() override; + + void PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance); + + void PublishEstimatorSelectorStatus(); + void PublishVehicleAttitude(); + void PublishVehicleLocalPosition(); + void PublishVehicleGlobalPosition(); + void PublishVehicleOdometry(); + void PublishWindEstimate(); + + bool SelectInstance(uint8_t instance); + + // Update the error scores for all available instances + bool UpdateErrorScores(); + + // Subscriptions (per estimator instance) + struct EstimatorInstance { + + EstimatorInstance(EKF2Selector *selector, uint8_t i) : + estimator_attitude_sub{selector, ORB_ID(estimator_attitude), i}, + estimator_status_sub{selector, ORB_ID(estimator_status), i}, + estimator_local_position_sub{ORB_ID(estimator_local_position), i}, + estimator_global_position_sub{ORB_ID(estimator_global_position), i}, + estimator_odometry_sub{ORB_ID(estimator_odometry), i}, + estimator_wind_sub{ORB_ID(estimator_wind), i}, + instance(i) + {} + + uORB::SubscriptionCallbackWorkItem estimator_attitude_sub; + uORB::SubscriptionCallbackWorkItem estimator_status_sub; + + uORB::Subscription estimator_local_position_sub; + uORB::Subscription estimator_global_position_sub; + uORB::Subscription estimator_odometry_sub; + uORB::Subscription estimator_wind_sub; + + uint64_t timestamp_sample_last{0}; + + uint32_t accel_device_id{0}; + uint32_t gyro_device_id{0}; + uint32_t baro_device_id{0}; + uint32_t mag_device_id{0}; + + hrt_abstime time_last_selected{0}; + + float combined_test_ratio{NAN}; + float relative_test_ratio{NAN}; + + bool healthy{false}; + bool filter_fault{false}; + bool timeout{false}; + + uint8_t healthy_count{0}; + + const uint8_t instance; + }; + + static constexpr float _rel_err_score_lim{1.0f}; // +- limit applied to the relative error score + static constexpr float _rel_err_thresh{0.5f}; // the relative score difference needs to be greater than this to switch from an otherwise healthy instance + + EstimatorInstance _instance[EKF2_MAX_INSTANCES] { + {this, 0}, + {this, 1}, +#if EKF2_MAX_INSTANCES > 2 + {this, 2}, + {this, 3}, +#if EKF2_MAX_INSTANCES > 4 + {this, 4}, + {this, 5}, + {this, 6}, + {this, 7}, + {this, 8}, +#endif +#endif + }; + + static constexpr uint8_t IMU_STATUS_SIZE = (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( + sensors_status_imu_s::gyro_inconsistency_rad_s[0])); + static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_gyro_error) / sizeof( + estimator_selector_status_s::accumulated_gyro_error[0]), + "increase estimator_selector_status_s::accumulated_gyro_error size"); + static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_accel_error) / sizeof( + estimator_selector_status_s::accumulated_accel_error[0]), + "increase estimator_selector_status_s::accumulated_accel_error size"); + static_assert(EKF2_MAX_INSTANCES <= sizeof(estimator_selector_status_s::combined_test_ratio) / sizeof( + estimator_selector_status_s::combined_test_ratio[0]), + "increase estimator_selector_status_s::combined_test_ratio size"); + + float _accumulated_gyro_error[IMU_STATUS_SIZE] {}; + float _accumulated_accel_error[IMU_STATUS_SIZE] {}; + hrt_abstime _last_update_us{0}; + bool _gyro_fault_detected{false}; + bool _accel_fault_detected{false}; + + uint8_t _available_instances{0}; + uint8_t _selected_instance{INVALID_INSTANCE}; + px4::atomic _request_instance{INVALID_INSTANCE}; + + uint32_t _instance_changed_count{0}; + hrt_abstime _last_instance_change{0}; + + hrt_abstime _last_status_publish{0}; + bool _selector_status_publish{false}; + + // vehicle_attitude: reset counters + vehicle_attitude_s _attitude_last{}; + matrix::Quatf _delta_q_reset{}; + uint8_t _quat_reset_counter{0}; + + // vehicle_local_position: reset counters + vehicle_local_position_s _local_position_last{}; + matrix::Vector2f _delta_xy_reset{}; + float _delta_z_reset{0.f}; + matrix::Vector2f _delta_vxy_reset{}; + float _delta_vz_reset{0.f}; + float _delta_heading_reset{0}; + uint8_t _xy_reset_counter{0}; + uint8_t _z_reset_counter{0}; + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _heading_reset_counter{0}; + + // vehicle_odometry + vehicle_odometry_s _odometry_last{}; + + // vehicle_global_position: reset counters + vehicle_global_position_s _global_position_last{}; + double _delta_lat_reset{0}; + double _delta_lon_reset{0}; + float _delta_alt_reset{0.f}; + uint8_t _lat_lon_reset_counter{0}; + uint8_t _alt_reset_counter{0}; + + // wind estimate + wind_s _wind_last{}; + + uint8_t _attitude_instance_prev{INVALID_INSTANCE}; + uint8_t _local_position_instance_prev{INVALID_INSTANCE}; + uint8_t _global_position_instance_prev{INVALID_INSTANCE}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)}; + + // Publications + uORB::Publication _estimator_selector_status_pub{ORB_ID(estimator_selector_status)}; + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; + uORB::Publication _vehicle_attitude_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; + uORB::Publication _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + uORB::Publication _wind_pub{ORB_ID(wind)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ekf2_sel_err_red, + (ParamFloat) _param_ekf2_sel_imu_angle_rate, + (ParamFloat) _param_ekf2_sel_imu_angle, + (ParamFloat) _param_ekf2_sel_imu_accel, + (ParamFloat) _param_ekf2_sel_imu_velocity + ) +}; diff --git a/src/prometheus_px4/src/modules/ekf2/Utility/CMakeLists.txt b/src/prometheus_px4/src/modules/ekf2/Utility/CMakeLists.txt new file mode 100644 index 0000000..7a635ac --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/Utility/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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(EKF2Utility + PreFlightChecker.cpp +) + +target_include_directories(EKF2Utility + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(EKF2Utility PRIVATE mathlib) + +px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS EKF2Utility) diff --git a/src/prometheus_px4/src/modules/ekf2/Utility/InnovationLpf.hpp b/src/prometheus_px4/src/modules/ekf2/Utility/InnovationLpf.hpp new file mode 100644 index 0000000..89964b0 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/Utility/InnovationLpf.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * First order "alpha" IIR digital filter with input saturation + */ + +#include + +class InnovationLpf final +{ +public: + InnovationLpf() = default; + ~InnovationLpf() = default; + + void reset(float val = 0.f) { _x = val; } + + /** + * Update the filter with a new value and returns the filtered state + * The new value is constained by the limit set in setSpikeLimit + * @param val new input + * @param alpha normalized weight of the new input + * @param spike_limit the amplitude of the saturation at the input of the filter + * @return filtered output + */ + float update(float val, float alpha, float spike_limit) + { + float val_constrained = math::constrain(val, -spike_limit, spike_limit); + float beta = 1.f - alpha; + + _x = beta * _x + alpha * val_constrained; + + return _x; + } + + /** + * Helper function to compute alpha from dt and the inverse of tau + * @param dt sampling time in seconds + * @param tau_inv inverse of the time constant of the filter + * @return alpha, the normalized weight of a new measurement + */ + static float computeAlphaFromDtAndTauInv(float dt, float tau_inv) + { + return math::constrain(dt * tau_inv, 0.f, 1.f); + } + +private: + float _x{}; ///< current state of the filter +}; diff --git a/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightChecker.cpp new file mode 100644 index 0000000..bdb7049 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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 PreFlightCheckHelper.cpp + * Class handling the EKF2 innovation pre flight checks + */ + +#include "PreFlightChecker.hpp" + +void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov) +{ + const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv); + + _has_heading_failed = preFlightCheckHeadingFailed(innov, alpha); + _has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha); + _has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha); + _has_height_failed = preFlightCheckHeightFailed(innov, alpha); +} + +bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha) +{ + const float heading_test_limit = selectHeadingTestLimit(); + const float heading_innov_spike_lim = 2.0f * heading_test_limit; + + const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim); + + return checkInnovFailed(heading_innov_lpf, innov.heading, heading_test_limit, heading_innov_spike_lim); +} + +float PreFlightChecker::selectHeadingTestLimit() +{ + // Select the max allowed heading innovaton depending on whether we are not aiding navigation using + // observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion). + const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding; + + return (is_ne_aiding && !_can_observe_heading_in_flight) + ? _nav_heading_innov_test_lim // more restrictive test limit + : _heading_innov_test_lim; // less restrictive test limit +} + +bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha) +{ + bool has_failed = false; + + if (_is_using_gps_aiding || _is_using_ev_vel_aiding) { + const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])), + fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1]))); + Vector2f vel_ne_innov_lpf; + vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim); + vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim); + has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim); + } + + if (_is_using_flow_aiding) { + const Vector2f flow_innov = Vector2f(innov.flow); + Vector2f flow_innov_lpf; + flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim); + flow_innov_lpf(1) = _filter_flow_x_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim); + has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim); + } + + return has_failed; +} + +bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha) +{ + const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel)); // only temporary solution + const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim); + return checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim); +} + +bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha) +{ + const float hgt_innov = fmaxf(fabsf(innov.gps_vpos), fmaxf(fabs(innov.ev_vpos), + fabs(innov.rng_vpos))); // only temporary solution + const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim); + return checkInnovFailed(hgt_innov_lpf, hgt_innov, _hgt_innov_test_lim, _hgt_innov_spike_lim); +} + +bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit, + const float spike_limit) +{ + return fabsf(innov_lpf) > test_limit || fabsf(innov) > spike_limit; +} + +bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, const float test_limit, + const float spike_limit) +{ + return innov_lpf.norm_squared() > sq(test_limit) + || innov.norm_squared() > sq(spike_limit); +} + +void PreFlightChecker::reset() +{ + _is_using_gps_aiding = false; + _is_using_flow_aiding = false; + _is_using_ev_pos_aiding = false; + _is_using_ev_vel_aiding = false; + _has_heading_failed = false; + _has_horiz_vel_failed = false; + _has_vert_vel_failed = false; + _has_height_failed = false; + _filter_vel_n_innov.reset(); + _filter_vel_e_innov.reset(); + _filter_vel_d_innov.reset(); + _filter_hgt_innov.reset(); + _filter_heading_innov.reset(); + _filter_flow_x_innov.reset(); + _filter_flow_y_innov.reset(); +} diff --git a/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightChecker.hpp new file mode 100644 index 0000000..07e6248 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * 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 PreFlightChecker.hpp + * Class handling the EKF2 innovation pre flight checks + * + * First call the update(...) function and then get the results + * using the hasXxxFailed() getters + */ + +#pragma once + +#include +#include + +#include + +#include "InnovationLpf.hpp" + +using matrix::Vector2f; + +class PreFlightChecker +{ +public: + PreFlightChecker() = default; + ~PreFlightChecker() = default; + + /* + * Reset all the internal states of the class to their default value + */ + void reset(); + + /* + * Update the internal states + * @param dt the sampling time + * @param innov the ekf2_innovation_s struct containing the current innovations + */ + void update(float dt, const estimator_innovations_s &innov); + + /* + * If set to true, the checker will use a less conservative heading innovation check + */ + void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; } + + void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; } + void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; } + void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; } + void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; } + + bool hasHeadingFailed() const { return _has_heading_failed; } + bool hasHorizVelFailed() const { return _has_horiz_vel_failed; } + bool hasVertVelFailed() const { return _has_vert_vel_failed; } + bool hasHeightFailed() const { return _has_height_failed; } + + /* + * Overall state of the pre fligh checks + * @return true if any of the check failed + */ + bool hasFailed() const { return hasHorizFailed() || hasVertFailed(); } + + /* + * Horizontal checks overall result + * @return true if one of the horizontal checks failed + */ + bool hasHorizFailed() const { return _has_heading_failed || _has_horiz_vel_failed; } + + /* + * Vertical checks overall result + * @return true if one of the vertical checks failed + */ + bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; } + + /* + * Check if the innovation fails the test + * To pass the test, the following conditions should be true: + * innov_lpf <= test_limit + * innov <= spike_limit + * @param innov_lpf the low-pass filtered innovation + * @param innov the current unfiltered innovation + * @param test_limit the magnitude test limit for innov_lpf + * @param spike_limit the magnitude test limit for innov + * @return true if the check failed the test, false otherwise + */ + static bool checkInnovFailed(float innov_lpf, float innov, float test_limit, float spike_limit); + + /* + * Check if the a innovation of a 2D vector fails the test + * To pass the test, the following conditions should be true: + * innov_lpf <= test_limit + * innov <= spike_limit + * @param innov_lpf the low-pass filtered innovation + * @param innov the current unfiltered innovation + * @param test_limit the magnitude test limit for innov_lpf + * @param spike_limit the magnitude test limit for innov + * @return true if the check failed the test, false otherwise + */ + static bool checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, float test_limit, float spike_limit); + + static constexpr float sq(float var) { return var * var; } + +private: + bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha); + float selectHeadingTestLimit(); + + bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha); + bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha); + bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha); + + bool _has_heading_failed{}; + bool _has_horiz_vel_failed{}; + bool _has_vert_vel_failed{}; + bool _has_height_failed{}; + + bool _can_observe_heading_in_flight{}; + bool _is_using_gps_aiding{}; + bool _is_using_flow_aiding{}; + bool _is_using_ev_pos_aiding{}; + bool _is_using_ev_vel_aiding{}; + + // Low-pass filters for innovation pre-flight checks + InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec) + InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec) + InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec) + InnovationLpf _filter_hgt_innov; ///< Preflight low pass filter height innovation (m) + InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad) + InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad) + InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad) + + // Preflight low pass filter time constant inverse (1/sec) + static constexpr float _innov_lpf_tau_inv = 0.2f; + // Maximum permissible velocity innovation to pass pre-flight checks (m/sec) + static constexpr float _vel_innov_test_lim = 0.5f; + // Maximum permissible height innovation to pass pre-flight checks (m) + static constexpr float _hgt_innov_test_lim = 1.5f; + // Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad) + static constexpr float _nav_heading_innov_test_lim = 0.25f; + // Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad) + static constexpr float _heading_innov_test_lim = 0.52f; + // Maximum permissible flow innovation to pass pre-flight checks + static constexpr float _flow_innov_test_lim = 0.25f; + // Preflight velocity innovation spike limit (m/sec) + static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; + // Preflight position innovation spike limit (m) + static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; + // Preflight flow innovation spike limit (rad) + static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim; +}; diff --git a/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp b/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp new file mode 100644 index 0000000..16e1bed --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Test code for PreFlightChecker class + * Run this test only using make tests TESTFILTER=PreFlightChecker + */ + +#include + +#include "PreFlightChecker.hpp" + +class PreFlightCheckerTest : public ::testing::Test +{ +}; + +TEST_F(PreFlightCheckerTest, testInnovFailed) +{ + const float test_limit = 1.0; ///< is the limit for innovation_lpf + const float spike_limit = 2.f * test_limit; ///< is the limit for innovation_lpf + const float innovations[9] = {0.0, 1.5, 2.5, -1.5, -2.5, 1.5, -1.5, -2.5, -2.5}; + const float innovations_lpf[9] = {0.0, 0.9, 0.9, -0.9, -0.9, 1.1, -1.1, -1.1, 1.1}; + const bool expected_result[9] = {false, false, true, false, true, true, true, true, true}; + + for (int i = 0; i < 9; i++) { + EXPECT_EQ(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit), + expected_result[i]); + } + + // Smaller test limit, all the checks should fail except the first + EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[0], innovations[0], 0.0, 0.0)); + + for (int i = 1; i < 9; i++) { + EXPECT_TRUE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 0.0, 0.0)); + } + + // Larger test limit, none of the checks should fail + for (int i = 0; i < 9; i++) { + EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 2.0, 4.0)); + } +} + +TEST_F(PreFlightCheckerTest, testInnov2dFailed) +{ + const float test_limit = 1.0; + const float spike_limit = 2.0; + Vector2f innovations[4] = {{0.0, 0.0}, {0.0, 0.0}, {0.0, -2.5}, {1.5, -1.5}}; + Vector2f innovations_lpf[4] = {{0.0, 0.0}, {1.1, 0.0}, {0.5, 0.5}, {1.0, -1.0}}; + const bool expected_result[4] = {false, true, true, true}; + + for (int i = 0; i < 4; i++) { + EXPECT_EQ(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit), + expected_result[i]); + } + + // Smaller test limit, all the checks should fail except the first + EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[0], innovations_lpf[0], 0.0, 0.0)); + + for (int i = 1; i < 4; i++) { + EXPECT_TRUE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 0.0, 0.0)); + } + + // Larger test limit, none of the checks should fail + for (int i = 0; i < 4; i++) { + EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 1.42, 2.84)); + } +} diff --git a/src/prometheus_px4/src/modules/ekf2/ekf2_params.c b/src/prometheus_px4/src/modules/ekf2/ekf2_params.c new file mode 100644 index 0000000..f31dc52 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/ekf2_params.c @@ -0,0 +1,1374 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2016 Estimation and Control Library (ECL). 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 ECL 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 ekf2_params.c + * Parameter definition for ekf2. + * + * @author Roman Bast + * + */ + +/** + * Minimum time of arrival delta between non-IMU observations before data is downsampled. + * + * Baro and Magnetometer data will be averaged before downsampling, other data will be point sampled resulting in loss of information. + * + * @group EKF2 + * @min 10 + * @max 50 + * @reboot_required true + * @unit ms + */ +PARAM_DEFINE_INT32(EKF2_MIN_OBS_DT, 20); + +/** + * Magnetometer measurement delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0); + +/** + * Barometer measurement delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0); + +/** + * GPS measurement delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 110); + +/** + * Optical flow measurement delay relative to IMU measurements + * + * Assumes measurement is timestamped at trailing edge of integration period + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_OF_DELAY, 20); + +/** + * Range finder measurement delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_DELAY, 5); + +/** + * Airspeed measurement delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 100); + +/** + * Vision Position Estimator delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 175); + +/** + * Auxillary Velocity Estimate (e.g from a landing target) delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5); + +/** + * Integer bitmask controlling GPS checks. + * + * Set bits to 1 to enable checks. Checks enabled by the following bit positions + * 0 : Minimum required sat count set by EKF2_REQ_NSATS + * 1 : Maximum allowed PDOP set by EKF2_REQ_PDOP + * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH + * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV + * 4 : Maximum allowed speed error set by EKF2_REQ_SACC + * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter. + * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter. + * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detecton of the stationary condition is controlled by the EKF2_MOVE_TEST parameter. + * 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT + * + * @group EKF2 + * @min 0 + * @max 511 + * @bit 0 Min sat count (EKF2_REQ_NSATS) + * @bit 1 Max PDOP (EKF2_REQ_PDOP) + * @bit 2 Max horizontal position error (EKF2_REQ_EPH) + * @bit 3 Max vertical position error (EKF2_REQ_EPV) + * @bit 4 Max speed error (EKF2_REQ_SACC) + * @bit 5 Max horizontal position rate (EKF2_REQ_HDRIFT) + * @bit 6 Max vertical position rate (EKF2_REQ_VDRIFT) + * @bit 7 Max horizontal speed (EKF2_REQ_HDRIFT) + * @bit 8 Max vertical velocity discrepancy (EKF2_REQ_VDRIFT) + */ +PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 245); + +/** + * Required EPH to use GPS. + * + * @group EKF2 + * @min 2 + * @max 100 + * @unit m + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 3.0f); + +/** + * Required EPV to use GPS. + * + * @group EKF2 + * @min 2 + * @max 100 + * @unit m + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 5.0f); + +/** + * Required speed accuracy to use GPS. + * + * @group EKF2 + * @min 0.5 + * @max 5.0 + * @unit m/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_SACC, 0.5f); + +/** + * Required satellite count to use GPS. + * + * @group EKF2 + * @min 4 + * @max 12 + */ +PARAM_DEFINE_INT32(EKF2_REQ_NSATS, 6); + +/** + * Maximum PDOP to use GPS. + * + * @group EKF2 + * @min 1.5 + * @max 5.0 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_PDOP, 2.5f); + +/** + * Maximum horizontal drift speed to use GPS. + * + * @group EKF2 + * @min 0.1 + * @max 1.0 + * @unit m/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.1f); + +/** + * Maximum vertical drift speed to use GPS. + * + * @group EKF2 + * @min 0.1 + * @max 1.5 + * @decimal 2 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.2f); + +/** + * Rate gyro noise for covariance prediction. + * + * @group EKF2 + * @min 0.0001 + * @max 0.1 + * @unit rad/s + * @decimal 4 + */ +PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.5e-2f); + +/** + * Accelerometer noise for covariance prediction. + * + * @group EKF2 + * @min 0.01 + * @max 1.0 + * @unit m/s^2 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 3.5e-1f); + +/** + * Process noise for IMU rate gyro bias prediction. + * + * @group EKF2 + * @min 0.0 + * @max 0.01 + * @unit rad/s^2 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 1.0e-3f); + +/** + * Process noise for IMU accelerometer bias prediction. + * + * @group EKF2 + * @min 0.0 + * @max 0.01 + * @unit m/s^3 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 3.0e-3f); + +/** + * Process noise for body magnetic field prediction. + * + * @group EKF2 + * @min 0.0 + * @max 0.1 + * @unit gauss/s + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 1.0e-4f); + +/** + * Process noise for earth magnetic field prediction. + * + * @group EKF2 + * @min 0.0 + * @max 0.1 + * @unit gauss/s + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f); + +/** + * Process noise for wind velocity prediction. + * + * @group EKF2 + * @min 0.0 + * @max 1.0 + * @unit m/s^2 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 1.0e-1f); + +/** + * Measurement noise for gps horizontal velocity. + * + * @group EKF2 + * @min 0.01 + * @max 5.0 + * @unit m/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.3f); + +/** + * Measurement noise for gps position. + * + * @group EKF2 + * @min 0.01 + * @max 10.0 + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 0.5f); + +/** + * Measurement noise for non-aiding position hold. + * + * @group EKF2 + * @min 0.5 + * @max 50.0 + * @unit m + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_NOAID_NOISE, 10.0f); + +/** + * Measurement noise for barometric altitude. + * + * @group EKF2 + * @min 0.01 + * @max 15.0 + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.5f); + +/** + * Measurement noise for magnetic heading fusion. + * + * @group EKF2 + * @min 0.01 + * @max 1.0 + * @unit rad + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 0.3f); + +/** + * Measurement noise for magnetometer 3-axis fusion. + * + * @group EKF2 + * @min 0.001 + * @max 1.0 + * @unit gauss + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f); + +/** + * Measurement noise for airspeed fusion. + * + * @group EKF2 + * @min 0.5 + * @max 5.0 + * @unit m/s + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f); + +/** + * Gate size for synthetic sideslip fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_BETA_GATE, 5.0f); + +/** + * Noise for synthetic sideslip fusion. + * + * @group EKF2 + * @min 0.1 + * @max 1.0 + * @unit m/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_BETA_NOISE, 0.3f); + +/** + * Gate size for magnetic heading fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 2.6f); + +/** + * Gate size for magnetometer XYZ component fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f); + +/** + * Integer bitmask controlling handling of magnetic declination. + * + * Set bits in the following positions to enable functions. + * 0 : Set to true to use the declination from the geo_lookup library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value. + * 1 : Set to true to save the EKF2_MAG_DECL parameter to the value returned by the EKF when the vehicle disarms. + * 2 : Set to true to always use the declination as an observation when 3-axis magnetometer fusion is being used. + * + * @group EKF2 + * @min 0 + * @max 7 + * @bit 0 use geo_lookup declination + * @bit 1 save EKF2_MAG_DECL on disarm + * @bit 2 use declination as an observation + * @reboot_required true + */ +PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7); + +/** + * Type of magnetometer fusion + * + * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-component vector. The fuson of magnetomer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field. + * If set to 'Automatic' magnetic heading fusion is used when on-ground and 3-axis magnetic field fusion in-flight with fallback to magnetic heading fusion if there is insufficient motion to make yaw or magnetic field states observable. + * If set to 'Magnetic heading' magnetic heading fusion is used at all times + * If set to '3-axis' 3-axis field fusion is used at all times. + * If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight. + * If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests controlled by the EKF2_MOVE_TEST parameter indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight. + * If set to 'None' the magnetometer will not be used under any circumstance. If no external source of yaw is available, it is possible to use post-takeoff horizontal movement combined with GPS velocity measurements to align the yaw angle with the timer required (depending on the amount of movement and GPS data quality). Other external sources of yaw may be used if selected via the EKF2_AID_MASK parameter. + * @group EKF2 + * @value 0 Automatic + * @value 1 Magnetic heading + * @value 2 3-axis + * @value 3 VTOL custom + * @value 4 MC custom + * @value 5 None + * @reboot_required true + */ +PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0); + +/** + * Horizontal acceleration threshold used by automatic selection of magnetometer fusion method. + * + * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered horizontal acceleration is greater than this parameter value, then the EKF will use 3-axis magnetomer fusion. + * + * @group EKF2 + * @min 0.0 + * @max 5.0 + * @unit m/s^2 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f); + +/** + * Yaw rate threshold used by automatic selection of magnetometer fusion method. + * + * This parameter is used when the magnetometer fusion method is set automatically (EKF2_MAG_TYPE = 0). If the filtered yaw rate is greater than this parameter value, then the EKF will use 3-axis magnetomer fusion. + * + * @group EKF2 + * @min 0.0 + * @max 1.0 + * @unit rad/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.25f); + +/** + * Gate size for barometric and GPS height fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f); + +/** + * Baro deadzone range for height fusion + * + * Sets the value of deadzone applied to negative baro innovations. + * Deadzone is enabled when EKF2_GND_EFF_DZ > 0. + * + * @group EKF2 + * @min 0.0 + * @max 10.0 + * @unit m + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GND_EFF_DZ, 4.0f); + +/** + * Height above ground level for ground effect zone + * + * Sets the maximum distance to the ground level where negative baro innovations are expected. + * + * @group EKF2 + * @min 0.0 + * @max 5.0 + * @unit m + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GND_MAX_HGT, 0.5f); + +/** + * Gate size for GPS horizontal position fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f); + +/** + * Gate size for GPS velocity fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f); + +/** + * Gate size for TAS fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); + +/** + * Integer bitmask controlling data fusion and aiding methods. + * + * Set bits in the following positions to enable: + * 0 : Set to true to use GPS data if available + * 1 : Set to true to use optical flow data if available + * 2 : Set to true to inhibit IMU delta velocity bias estimation + * 3 : Set to true to enable vision position fusion + * 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true. + * 5 : Set to true to enable multi-rotor drag specific force fusion + * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used + * 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true. + * + * @group EKF2 + * @min 0 + * @max 511 + * @bit 0 use GPS + * @bit 1 use optical flow + * @bit 2 inhibit IMU bias estimation + * @bit 3 vision position fusion + * @bit 4 vision yaw fusion + * @bit 5 multi-rotor drag fusion + * @bit 6 rotate external vision + * @bit 7 GPS yaw fusion + * @bit 8 vision velocity fusion + * @reboot_required true + */ +PARAM_DEFINE_INT32(EKF2_AID_MASK, 1); + +/** + * Determines the primary source of height data used by the EKF. + * + * The range sensor option should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level. + * + * @group EKF2 + * @value 0 Barometric pressure + * @value 1 GPS + * @value 2 Range sensor + * @value 3 Vision + * @reboot_required true + */ +PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0); + +/** + * Integer bitmask controlling fusion sources of the terrain estimator + * + * Set bits in the following positions to enable: + * 0 : Set to true to use range finder data if available + * 1 : Set to true to use optical flow data if available + * + * @group EKF2 + * @min 0 + * @max 3 + * @bit 0 use range finder + * @bit 1 use optical flow + */ +PARAM_DEFINE_INT32(EKF2_TERR_MASK, 3); + +/** + * Maximum lapsed time from last fusion of measurements that constrain velocity drift before the EKF will report the horizontal nav solution as invalid. + * + * @group EKF2 + * @group EKF2 + * @min 500000 + * @max 10000000 + * @unit us + */ +PARAM_DEFINE_INT32(EKF2_NOAID_TOUT, 5000000); + +/** + * Measurement noise for range finder fusion + * + * @group EKF2 + * @min 0.01 + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_NOISE, 0.1f); + +/** + * Range finder range dependant noise scaler. + * + * Specifies the increase in range finder noise with range. + * + * @group EKF2 + * @min 0.0 + * @max 0.2 + * @unit m/m + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_SFE, 0.05f); + +/** + * Gate size for range finder fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); + +/** + * Expected range finder reading when on ground. + * + * If the vehicle is on ground, is not moving as determined by the motion test controlled by EKF2_MOVE_TEST and the range finder is returning invalid or no data, then an assumed range value of EKF2_MIN_RNG will be used by the terrain estimator so that a terrain height estimate is avilable at the start of flight in situations where the range finder may be inside its minimum measurements distance when on ground. + * + * @group EKF2 + * @min 0.01 + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); + +/** + * Whether to set the external vision observation noise from the parameter or from vision message + * + * If set to true the observation noise is set from the parameters directly, if set to false the measurement noise is taken from the vision message and the parameter are used as a lower bound. + * + * @boolean + * @group EKF2 + */ +PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0); + +/** + * Measurement noise for vision position observations used to lower bound or replace the uncertainty included in the message + * + * @group EKF2 + * @min 0.01 + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.1f); + +/** + * Measurement noise for vision velocity observations used to lower bound or replace the uncertainty included in the message + * + * @group EKF2 + * @min 0.01 + * @unit m/s + * @decimal 2 +*/ +PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f); + +/** + * Measurement noise for vision angle observations used to lower bound or replace the uncertainty included in the message + * + * @group EKF2 + * @min 0.05 + * @unit rad + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f); + +/** + * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum + * + * @group EKF2 + * @min 0.05 + * @unit rad/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f); + +/** + * Measurement noise for the optical flow sensor. + * + * (when it's reported quality metric is at the minimum set by EKF2_OF_QMIN). + * The following condition must be met: EKF2_OF_N_MAXN >= EKF2_OF_N_MIN + * + * @group EKF2 + * @min 0.05 + * @unit rad/s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); + +/** + * Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN. + * + * @group EKF2 + * @min 0 + * @max 255 + */ +PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1); + +/** + * Gate size for optical flow fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_OF_GATE, 3.0f); + +/** + * Terrain altitude process noise - accounts for instability in vehicle height estimate + * + * @group EKF2 + * @min 0.5 + * @unit m/s + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_TERR_NOISE, 5.0f); + +/** + * Magnitude of terrain gradient + * + * @group EKF2 + * @min 0.0 + * @unit m/m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_TERR_GRAD, 0.5f); + +/** + * X position of IMU in body frame (forward axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_IMU_POS_X, 0.0f); + +/** + * Y position of IMU in body frame (right axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Y, 0.0f); + +/** + * Z position of IMU in body frame (down axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Z, 0.0f); + +/** + * X position of GPS antenna in body frame (forward axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_POS_X, 0.0f); + +/** + * Y position of GPS antenna in body frame (right axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Y, 0.0f); + +/** + * Z position of GPS antenna in body frame (down axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Z, 0.0f); + +/** + * X position of range finder origin in body frame (forward axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_POS_X, 0.0f); + +/** + * Y position of range finder origin in body frame (right axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_POS_Y, 0.0f); + +/** + * Z position of range finder origin in body frame (down axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_POS_Z, 0.0f); + +/** + * X position of optical flow focal point in body frame (forward axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_OF_POS_X, 0.0f); + +/** + * Y position of optical flow focal point in body frame (right axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f); + +/** + * Z position of optical flow focal point in body frame (down axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f); + +/** +* X position of VI sensor focal point in body frame (forward axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_POS_X, 0.0f); + +/** + * Y position of VI sensor focal point in body frame (right axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f); + +/** + * Z position of VI sensor focal point in body frame (down axis with origin relative to vehicle centre of gravity) + * + * @group EKF2 + * @unit m + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f); + +/** +* Airspeed fusion threshold. +* +* A value of zero will deactivate airspeed fusion. Any other positive +* value will determine the minimum airspeed which will still be fused. Set to about 90% of the vehicles stall speed. +* Both airspeed fusion and sideslip fusion must be active for the EKF to continue navigating after loss of GPS. +* Use EKF2_FUSE_BETA to activate sideslip fusion. +* +* @group EKF2 +* @min 0.0 +* @unit m/s +* @decimal 1 +*/ +PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f); + +/** +* Boolean determining if synthetic sideslip measurements should fused. +* +* A value of 1 indicates that fusion is active +* Both sideslip fusion and airspeed fusion must be active for the EKF to continue navigating after loss of GPS. +* Use EKF2_ARSP_THR to activate airspeed fusion. +* +* @group EKF2 +* @boolean +*/ +PARAM_DEFINE_INT32(EKF2_FUSE_BETA, 0); + +/** + + * Time constant of the velocity output prediction and smoothing filter + * + * @group EKF2 + * @max 1.0 + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.25f); + +/** + * Time constant of the position output prediction and smoothing filter. Controls how tightly the output track the EKF states. + * + * @group EKF2 + * @min 0.1 + * @max 1.0 + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_TAU_POS, 0.25f); + +/** + * 1-sigma IMU gyro switch-on bias + * + * @group EKF2 + * @min 0.0 + * @max 0.2 + * @unit rad/s + * @reboot_required true + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_GBIAS_INIT, 0.1f); + +/** + * 1-sigma IMU accelerometer switch-on bias + * + * @group EKF2 + * @min 0.0 + * @max 0.5 + * @unit m/s^2 + * @reboot_required true + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f); + +/** + * 1-sigma tilt angle uncertainty after gravity vector alignment + * + * @group EKF2 + * @min 0.0 + * @max 0.5 + * @unit rad + * @reboot_required true + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f); + +/** + * Range sensor pitch offset. + * + * @group EKF2 + * @min -0.75 + * @max 0.75 + * @unit rad + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f); + +/** + * Range sensor aid. + * + * If this parameter is enabled then the estimator will make use of the range finder measurements + * to estimate it's height even if range sensor is not the primary height source. It will only do so if conditions + * for range measurement fusion are met. This enables the range finder to be used during low speed and low altitude + * operation, eg takeoff and landing, where baro interference from rotor wash is excessive and can corrupt EKF state + * estimates. It is intended to be used where a vertical takeoff and landing is performed, and horizontal flight does + * not occur until above EKF2_RNG_A_HMAX. If vehicle motion causes repeated switching between the primary height + * sensor and range finder, an offset in the local position origin can accumulate. Also range finder measurements + * are less reliable and can experience unexpected errors. For these reasons, if accurate control of height + * relative to ground is required, it is recommended to use the MPC_ALT_MODE parameter instead, unless baro errors + * are severe enough to cause problems with landing and takeoff. + * + * @group EKF2 + * @value 0 Range aid disabled + * @value 1 Range aid enabled + */ +PARAM_DEFINE_INT32(EKF2_RNG_AID, 1); + +/** + * Maximum horizontal velocity allowed for range aid mode. + * + * If the vehicle horizontal speed exceeds this value then the estimator will not fuse range measurements + * to estimate it's height. This only applies when range aid mode is activated (EKF2_RNG_AID = enabled). + * + * @group EKF2 + * @min 0.1 + * @max 2 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 1.0f); + +/** + * Maximum absolute altitude (height above ground level) allowed for range aid mode. + * + * If the vehicle absolute altitude exceeds this value then the estimator will not fuse range measurements + * to estimate it's height. This only applies when range aid mode is activated (EKF2_RNG_AID = enabled). + * + * @group EKF2 + * @min 1.0 + * @max 10.0 + * @unit m + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f); + +/** + * Gate size used for innovation consistency checks for range aid fusion + * + * A lower value means HAGL needs to be more stable in order to use range finder for height estimation + * in range aid mode + * + * @group EKF2 + * @unit SD + * @min 0.1 + * @max 5.0 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f); + +/** + * Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) + * + * + * @group EKF2 + * @unit s + * @min 0.1 + * @max 5 +*/ +PARAM_DEFINE_FLOAT(EKF2_RNG_QLTY_T, 1.0f); + +/** + * Gate size for vision velocity estimate fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 +*/ +PARAM_DEFINE_FLOAT(EKF2_EVV_GATE, 3.0f); + +/** + * Gate size for vision position fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 +*/ +PARAM_DEFINE_FLOAT(EKF2_EVP_GATE, 5.0f); + +/** + * Specific drag force observation noise variance used by the multi-rotor specific drag force model. + * + * Increasing this makes the multi-rotor wind estimates adjust more slowly. + * + * @group EKF2 + * @min 0.5 + * @max 10.0 + * @unit (m/s^2)^2 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_DRAG_NOISE, 2.5f); + +/** + * X-axis ballistic coefficient used by the multi-rotor specific drag force model. + * + * This should be adjusted to minimise variance of the X-axis drag specific force innovation sequence. + * + * @group EKF2 + * @min 1.0 + * @max 100.0 + * @unit kg/m^2 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_BCOEF_X, 25.0f); + +/** + * Y-axis ballistic coefficient used by the multi-rotor specific drag force model. + * + * This should be adjusted to minimise variance of the Y-axis drag specific force innovation sequence. + * + * @group EKF2 + * @min 1.0 + * @max 100.0 + * @unit kg/m^2 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_BCOEF_Y, 25.0f); + +/** + * Upper limit on airspeed along individual axes used to correct baro for position error effects + * + * @group EKF2 + * @min 5.0 + * @max 50.0 + * @unit m/s + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_ASPD_MAX, 20.0f); + +/** + * Static pressure position error coefficient for the positive X axis + * + * This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. + * If the baro height estimate rises during forward flight, then this will be a negative number. + * + * @group EKF2 + * @min -0.5 + * @max 0.5 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_PCOEF_XP, 0.0f); + +/** + * Static pressure position error coefficient for the negative X axis. + * + * This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. + * If the baro height estimate rises during backwards flight, then this will be a negative number. + * + * @group EKF2 + * @min -0.5 + * @max 0.5 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_PCOEF_XN, 0.0f); + +/** + * Pressure position error coefficient for the positive Y axis. + * + * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the positive Y (RH) body axis. + * If the baro height estimate rises during sideways flight to the right, then this will be a negative number. + * + * @group EKF2 + * @min -0.5 + * @max 0.5 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_PCOEF_YP, 0.0f); + +/** + * Pressure position error coefficient for the negative Y axis. + * + * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the negative Y (LH) body axis. + * If the baro height estimate rises during sideways flight to the left, then this will be a negative number. + * + * @group EKF2 + * @min -0.5 + * @max 0.5 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_PCOEF_YN, 0.0f); + +/** + * Static pressure position error coefficient for the Z axis. + * + * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the Z body axis. + * + * @group EKF2 + * @min -0.5 + * @max 0.5 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_PCOEF_Z, 0.0f); + +/** + * Accelerometer bias learning limit. + * + * The ekf delta velocity bias states will be limited to within a range equivalent to +- of this value. + * + * @group EKF2 + * @min 0.0 + * @max 0.8 + * @unit m/s^2 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_ABL_LIM, 0.4f); + +/** + * Maximum IMU accel magnitude that allows IMU bias learning. + * + * If the magnitude of the IMU accelerometer vector exceeds this value, the EKF delta velocity state estimation will be inhibited. + * This reduces the adverse effect of high manoeuvre accelerations and IMU nonlinerity and scale factor errors on the delta velocity bias estimates. + * + * @group EKF2 + * @min 20.0 + * @max 200.0 + * @unit m/s^2 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_ABL_ACCLIM, 25.0f); + +/** + * Maximum IMU gyro angular rate magnitude that allows IMU bias learning. + * + * If the magnitude of the IMU angular rate vector exceeds this value, the EKF delta velocity state estimation will be inhibited. + * This reduces the adverse effect of rapid rotation rates and associated errors on the delta velocity bias estimates. + * + * @group EKF2 + * @min 2.0 + * @max 20.0 + * @unit rad/s + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f); + +/** + * Time constant used by acceleration and angular rate magnitude checks used to inhibit delta velocity bias learning. + * + * The vector magnitude of angular rate and acceleration used to check if learning should be inhibited has a peak hold filter applied to it with an exponential decay. + * This parameter controls the time constant of the decay. + * + * @group EKF2 + * @min 0.1 + * @max 1.0 + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f); + +/** + * Vehicle movement test threshold + * + * Scales the threshold tests applied to IMU data used to determine if the vehicle is static or moving. See parameter descriptions for EKF2_GPS_CHECK and EKF2_MAG_TYPE for further information on the functionality affected by this parameter. + * + * @group EKF2 + * @min 0.1 + * @max 10.0 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_MOVE_TEST, 1.0f); + +/** + * Required GPS health time on startup + * + * Minimum continuous period without GPS failure required to mark a healthy GPS status. + * It can be reduced to speed up initialization, but it's recommended to keep this unchanged for a vehicle. + * + * @group EKF2 + * @min 0.1 + * @decimal 1 + * @unit s + * @reboot_required true + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f); + +/** + * Magnetic field strength test selection + * + * When set, the EKF checks the strength of the magnetic field + * to decide whether the magnetometer data is valid. + * If GPS data is received, the magnetic field is compared to a World + * Magnetic Model (WMM), otherwise an average value is used. + * This check is useful to reject occasional hard iron disturbance. + * + * @group EKF2 + * @boolean + */ +PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 0); + +/** + * Enable synthetic magnetometer Z component measurement. + * + * Use for vehicles where the measured body Z magnetic field is subject to strong magnetic interference. + * For magnetic heading fusion the magnetometer Z measurement will be replaced by a synthetic value calculated + * using the knowledge of the 3D magnetic field vector at the location of the drone. Therefore, this parameter + * will only have an effect if the global position of the drone is known. + * For 3D mag fusion the magnetometer Z measurement will simply be ingored instead of fusing the synthetic value. + * + * @group EKF2 + * @boolean +*/ +PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0); + +/** + * Default value of true airspeed used in EKF-GSF AHRS calculation. + * + * If no airspeed measurements are avalable, the EKF-GSF AHRS calculation will assume this value of true airspeed when compensating for centripetal acceleration during turns. Set to zero to disable centripetal acceleration compensation during fixed wing flight modes. + * + * @group EKF2 + * @min 0.0 + * @unit m/s + * @max 100.0 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f); diff --git a/src/prometheus_px4/src/modules/ekf2/ekf2_params_multi.c b/src/prometheus_px4/src/modules/ekf2/ekf2_params_multi.c new file mode 100644 index 0000000..cd06198 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/ekf2_params_multi.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Multi-EKF IMUs + * + * Maximum number of IMUs to use for Multi-EKF. Set 0 to disable. + * Requires SENS_IMU_MODE 0. + * + * @group EKF2 + * @reboot_required true + * @min 0 + * @max 4 + */ +PARAM_DEFINE_INT32(EKF2_MULTI_IMU, 0); + +/** + * Multi-EKF Magnetometers. + * + * Maximum number of magnetometers to use for Multi-EKF. Set 0 to disable. + * Requires SENS_MAG_MODE 0. + * + * @group EKF2 + * @reboot_required true + * @min 0 + * @max 4 + */ +PARAM_DEFINE_INT32(EKF2_MULTI_MAG, 0); diff --git a/src/prometheus_px4/src/modules/ekf2/ekf2_params_selector.c b/src/prometheus_px4/src/modules/ekf2/ekf2_params_selector.c new file mode 100644 index 0000000..43a630f --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/ekf2_params_selector.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Selector error reduce threshold + * + * EKF2 instances have to be better than the selected by at least this amount before their relative score can be reduced. + * + * @group EKF2 + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_ERR_RED, 0.2f); + +/** + * Selector angular rate threshold + * + * EKF2 selector angular rate error threshold for comparing gyros. Angular rate vector differences larger than this will result in accumulated angular error. + * + * @group EKF2 + * @unit deg/s + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_RAT, 7.0f); + +/** + * Selector angular threshold. + * + * EKF2 selector maximum accumulated angular error threshold for comparing gyros. Accumulated angular error larger than this will result in the sensor being declared faulty. + * + * @group EKF2 + * @unit deg + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ANG, 15.0f); + +/** + * Selector acceleration threshold + * + * EKF2 selector acceleration error threshold for comparing accelerometers. Acceleration vector differences larger than this will result in accumulated velocity error. + * + * @group EKF2 + * @unit m/s^2 + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f); + +/** + * Selector angular threshold. + * + * EKF2 selector maximum accumulated velocity threshold for comparing accelerometers. Accumulated velocity error larger than this will result in the sensor being declared faulty. + * + * @group EKF2 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f); diff --git a/src/prometheus_px4/src/modules/ekf2/ekf2_params_volatile.c b/src/prometheus_px4/src/modules/ekf2/ekf2_params_volatile.c new file mode 100644 index 0000000..5b947a5 --- /dev/null +++ b/src/prometheus_px4/src/modules/ekf2/ekf2_params_volatile.c @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Magnetic declination + * + * @group EKF2 + * @volatile + * @category system + * @unit deg + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0); diff --git a/src/prometheus_px4/src/modules/esc_battery/CMakeLists.txt b/src/prometheus_px4/src/modules/esc_battery/CMakeLists.txt new file mode 100644 index 0000000..3ff9b52 --- /dev/null +++ b/src/prometheus_px4/src/modules/esc_battery/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__esc_battery + MAIN esc_battery + COMPILE_FLAGS + SRCS + EscBattery.cpp + EscBattery.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/esc_battery/EscBattery.cpp b/src/prometheus_px4/src/modules/esc_battery/EscBattery.cpp new file mode 100644 index 0000000..9ad04b0 --- /dev/null +++ b/src/prometheus_px4/src/modules/esc_battery/EscBattery.cpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * 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 "EscBattery.hpp" + +using namespace time_literals; + +EscBattery::EscBattery() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::lp_default), + _battery(1, this, ESC_BATTERY_INTERVAL_US) +{ +} + +bool +EscBattery::init() +{ + if (!_esc_status_sub.registerCallback()) { + PX4_ERR("esc_status callback registration failed!"); + return false; + } + + _esc_status_sub.set_interval_us(ESC_BATTERY_INTERVAL_US); + + return true; +} + +void +EscBattery::parameters_updated() +{ + ModuleParams::updateParams(); +} + +void +EscBattery::Run() +{ + if (should_exit()) { + _esc_status_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + if (_parameter_update_sub.updated()) { + // Clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + parameters_updated(); + } + + esc_status_s esc_status; + + if (_esc_status_sub.copy(&esc_status)) { + + int online_bitmask = (1 << esc_status.esc_count) - 1; + + if (online_bitmask != esc_status.esc_online_flags || esc_status.esc_count == 0 || + esc_status.esc_count > esc_status_s::CONNECTED_ESC_MAX) { + return; + } + + float average_voltage_v = 0.0f; + float total_current_a = 0.0f; + + for (unsigned i = 0; i < esc_status.esc_count; ++i) { + average_voltage_v += esc_status.esc[i].esc_voltage; + total_current_a += esc_status.esc[i].esc_current; + } + + average_voltage_v /= esc_status.esc_count; + + const bool connected = true; + const int priority = 0; + + actuator_controls_s ctrl; + _actuator_ctrl_0_sub.copy(&ctrl); + + _battery.updateBatteryStatus( + esc_status.timestamp, + average_voltage_v, + total_current_a, + connected, + battery_status_s::BATTERY_SOURCE_ESCS, + priority, + ctrl.control[actuator_controls_s::INDEX_THROTTLE]); + } +} + +int EscBattery::task_spawn(int argc, char *argv[]) +{ + EscBattery *instance = new EscBattery(); + + 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 EscBattery::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int EscBattery::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements using information from the ESC status and publish it as battery status. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("esc_battery", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int esc_battery_main(int argc, char *argv[]) +{ + return EscBattery::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/esc_battery/EscBattery.hpp b/src/prometheus_px4/src/modules/esc_battery/EscBattery.hpp new file mode 100644 index 0000000..dc0ae3a --- /dev/null +++ b/src/prometheus_px4/src/modules/esc_battery/EscBattery.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class EscBattery : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + EscBattery(); + ~EscBattery() = 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 parameters_updated(); + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; + uORB::SubscriptionCallbackWorkItem _esc_status_sub{this, ORB_ID(esc_status)}; + + static constexpr uint32_t ESC_BATTERY_INTERVAL_US = 20_ms; // assume higher frequency esc feedback than 50Hz + Battery _battery; +}; diff --git a/src/prometheus_px4/src/modules/events/CMakeLists.txt b/src/prometheus_px4/src/modules/events/CMakeLists.txt new file mode 100644 index 0000000..8c54833 --- /dev/null +++ b/src/prometheus_px4/src/modules/events/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +px4_add_module( + MODULE modules__events + MAIN send_event + COMPILE_FLAGS + SRCS + rc_loss_alarm.cpp + send_event.cpp + set_leds.cpp + status_display.cpp + ) diff --git a/src/prometheus_px4/src/modules/events/events_params.c b/src/prometheus_px4/src/modules/events/events_params.c new file mode 100644 index 0000000..d9336bf --- /dev/null +++ b/src/prometheus_px4/src/modules/events/events_params.c @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 events_params.c + * + * Parameters defined by the events module. + */ + +#include +#include + +/** + * Status Display + * + * Enable/disable event task for displaying the vehicle status using arm-mounted + * LEDs. When enabled and if the vehicle supports it, LEDs will flash + * indicating various vehicle status changes. Currently PX4 has not implemented + * any specific status events. + * - + * + * @group Events + * @boolean + * @reboot_required true + */ +PARAM_DEFINE_INT32(EV_TSK_STAT_DIS, 0); + +/** + * RC Loss Alarm + * + * Enable/disable event task for RC Loss. When enabled, an alarm tune will be + * played via buzzer or ESCs, if supported. The alarm will sound after a disarm, + * if the vehicle was previously armed and only if the vehicle had RC signal at + * some point. Particularly useful for locating crashed drones without a GPS + * sensor. + * + * @group Events + * @boolean + * @reboot_required true + */ +PARAM_DEFINE_INT32(EV_TSK_RC_LOSS, 0); diff --git a/src/prometheus_px4/src/modules/events/rc_loss_alarm.cpp b/src/prometheus_px4/src/modules/events/rc_loss_alarm.cpp new file mode 100644 index 0000000..a1df1c3 --- /dev/null +++ b/src/prometheus_px4/src/modules/events/rc_loss_alarm.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 rc_loss_alarm.cpp + * + */ + +#include "rc_loss_alarm.h" + +#include + +#include +#include + +#include + +namespace events +{ +namespace rc_loss +{ + +void RC_Loss_Alarm::process() +{ + vehicle_status_s status{}; + + if (!_vehicle_status_sub.update(&status)) { + return; + } + + if (!_was_armed && + status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + _was_armed = true; // Once true, impossible to go back to false + } + + if (!_had_rc && !status.rc_signal_lost) { + + _had_rc = true; + } + + if (_was_armed && _had_rc && status.rc_signal_lost && + status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + play_tune(); + _alarm_playing = true; + + } else if (_alarm_playing) { + stop_tune(); + _alarm_playing = false; + } +} + +void RC_Loss_Alarm::play_tune() +{ + tune_control_s tune_control{}; + tune_control.tune_id = tune_control_s::TUNE_ID_ERROR; + tune_control.tune_override = true; + tune_control.volume = tune_control_s::VOLUME_LEVEL_MAX; + tune_control.timestamp = hrt_absolute_time(); + _tune_control_pub.publish(tune_control); +} + +void RC_Loss_Alarm::stop_tune() +{ + tune_control_s tune_control{}; + tune_control.tune_override = true; + tune_control.timestamp = hrt_absolute_time(); + _tune_control_pub.publish(tune_control); +} + +} /* namespace rc_loss */ +} /* namespace events */ diff --git a/src/prometheus_px4/src/modules/events/rc_loss_alarm.h b/src/prometheus_px4/src/modules/events/rc_loss_alarm.h new file mode 100644 index 0000000..861ced0 --- /dev/null +++ b/src/prometheus_px4/src/modules/events/rc_loss_alarm.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 rc_loss_alarm.h + * Tone alarm in the event of RC loss + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace events +{ +namespace rc_loss +{ + +class RC_Loss_Alarm +{ +public: + + RC_Loss_Alarm() = default; + + /** regularily called to handle state updates */ + void process(); + +private: + /** Publish tune control to sound alarm */ + void play_tune(); + + /** Publish tune control to interrupt any sound */ + void stop_tune(); + + uORB::Publication _tune_control_pub{ORB_ID(tune_control)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + bool _was_armed = false; + bool _had_rc = false; // Don't trigger alarm for systems without RC + bool _alarm_playing = false; +}; + +} /* namespace rc_loss */ +} /* namespace events */ diff --git a/src/prometheus_px4/src/modules/events/send_event.cpp b/src/prometheus_px4/src/modules/events/send_event.cpp new file mode 100644 index 0000000..bc92d44 --- /dev/null +++ b/src/prometheus_px4/src/modules/events/send_event.cpp @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "send_event.h" + +#include + +#include +#include +#include + +using namespace time_literals; + +namespace events +{ + +// Run it at 30 Hz. +static constexpr uint32_t SEND_EVENT_INTERVAL_US{1_s / 30}; + +int SendEvent::task_spawn(int argc, char *argv[]) +{ + SendEvent *send_event = new SendEvent(); + + if (!send_event) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(send_event); + _task_id = task_id_is_work_queue; + + send_event->start(); + + return 0; +} + +SendEvent::SendEvent() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + if (_param_ev_tsk_stat_dis.get()) { + _status_display = new status::StatusDisplay(); + } + + if (_param_ev_tsk_rc_loss.get()) { + _rc_loss_alarm = new rc_loss::RC_Loss_Alarm(); + } +} + +SendEvent::~SendEvent() +{ + ScheduleClear(); + + delete _status_display; + delete _rc_loss_alarm; +} + +int SendEvent::start() +{ + ScheduleOnInterval(SEND_EVENT_INTERVAL_US, 10000); + + return PX4_OK; +} + +void SendEvent::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + process_commands(); + + if (_status_display != nullptr) { + _status_display->process(); + } + + if (_rc_loss_alarm != nullptr) { + _rc_loss_alarm->process(); + } +} + +void SendEvent::process_commands() +{ + // TODO: do something with vehicle commands + // TODO: what is this modules purpose? +} + +void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result) +{ + /* publish ACK */ + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = cmd.command; + command_ack.result = (uint8_t)result; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + + uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); +} + +int SendEvent::print_usage(const char *reason) +{ + if (reason) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Background process running periodically on the LP work queue to perform housekeeping tasks. +It is currently only responsible for tone alarm on RC Loss. + +The tasks can be started via CLI or uORB topics (vehicle_command from MAVLink, etc.). +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("send_event", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int SendEvent::custom_command(int argc, char *argv[]) +{ + // TODO: what is my purpose? + print_usage("unrecognized command"); + + return 0; +} + +int send_event_main(int argc, char *argv[]) +{ + return SendEvent::main(argc, argv); +} + +} /* namespace events */ diff --git a/src/prometheus_px4/src/modules/events/send_event.h b/src/prometheus_px4/src/modules/events/send_event.h new file mode 100644 index 0000000..267ab9f --- /dev/null +++ b/src/prometheus_px4/src/modules/events/send_event.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include "status_display.h" +#include "rc_loss_alarm.h" + +#include +#include +#include +#include +#include +#include + +namespace events +{ + +extern "C" __EXPORT int send_event_main(int argc, char *argv[]); + +/** @class SendEvent The SendEvent class manages the RC loss audible alarm, LED status display, and thermal calibration. */ +class SendEvent : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + SendEvent(); + + ~SendEvent(); + + /** + * @see ModuleBase + * @brief Recognizes custom startup commands, called from the main() function entry. + * @param argc The task argument count. + * @param argc Pointer to the task argument variable array. + * @return Returns 0 iff successful, otherwise < 0 on error. + */ + static int custom_command(int argc, char *argv[]); + + /** + * @see ModuleBase + * @brief Prints usage options to the console. + * @param reason The requested usage reason for printing to console. + * @return Returns 0 iff successful, -1 otherwise. + */ + static int print_usage(const char *reason = nullptr); + + /** + * @brief Spawns and initializes the class in the same context as the + * work queue and starts the background listener. + * @param argc The input argument count. + * @param argv Pointer to the input argument array. + * @return Returns 0 iff successful, -1 otherwise. + */ + static int task_spawn(int argc, char *argv[]); + +private: + + /** + * @brief Returns an ACK to a vehicle_command. + * @param cmd The vehicle command struct being referenced. + * @param result The command acknowledgement result. + */ + void answer_command(const vehicle_command_s &cmd, unsigned result); + + /** + * @brief Calls process_commands() and schedules the next cycle. + */ + void Run() override; + + /** + * @brief Checks for new commands and processes them. + */ + void process_commands(); + + /** + * @brief Starts background task listening for commands. + * @return Returns 0 iff successful, otherwise < 0 on error. + */ + int start(); + + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + + /** @var _status_display Pointer to the status display object. */ + status::StatusDisplay *_status_display = nullptr; + + /** @var _rc_loss_alarm Pointer to the RC loss alarm object. */ + rc_loss::RC_Loss_Alarm *_rc_loss_alarm = nullptr; + + /** @note Declare local parameters using defined parameters. */ + DEFINE_PARAMETERS( + /** @var _param_status_display Parameter to enable/disable the LED status display. */ + (ParamBool) _param_ev_tsk_stat_dis, + + /** @var _param_rc_loss The RC comms loss status flag. */ + (ParamBool) _param_ev_tsk_rc_loss + ) +}; + +} // namespace events diff --git a/src/prometheus_px4/src/modules/events/set_leds.cpp b/src/prometheus_px4/src/modules/events/set_leds.cpp new file mode 100644 index 0000000..ed441f5 --- /dev/null +++ b/src/prometheus_px4/src/modules/events/set_leds.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 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 set_leds.cpp + * Separate the set_leds() function + * + * @author Christoph Tobler + */ + +#include "status_display.h" + +namespace events +{ +namespace status +{ + +void StatusDisplay::set_leds() +{ + // Put your LED handling here +} + +} /* namespace status */ +} /* namespace events */ diff --git a/src/prometheus_px4/src/modules/events/status_display.cpp b/src/prometheus_px4/src/modules/events/status_display.cpp new file mode 100644 index 0000000..b4fcc6b --- /dev/null +++ b/src/prometheus_px4/src/modules/events/status_display.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 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 status_display.cpp + * Status Display: this decouples the LED and tune logic from the control logic in commander + * + * @author Simone Guscetti + * @author Beat Küng + * @editor Christoph Tobler + */ + +#include "status_display.h" +#include + +namespace events +{ +namespace status +{ + +StatusDisplay::StatusDisplay() +{ + // set the base color + _led_control.priority = 0; + _led_control.led_mask = 0xff; + _led_control.color = led_control_s::COLOR_CYAN; + _led_control.mode = led_control_s::MODE_ON; + publish(); + + _led_control.priority = 1; + _led_control.num_blinks = 0; // infinite blinking +} + +bool StatusDisplay::check_for_updates() +{ + bool got_updates = false; + + if (_battery_status_sub.update()) { + got_updates = true; + } + + if (_cpu_load_sub.update()) { + got_updates = true; + } + + if (_vehicle_status_flags_sub.update()) { + got_updates = true; + } + + if (_vehicle_status_sub.update()) { + got_updates = true; + } + + return got_updates; +} + +void StatusDisplay::process() +{ + if (!check_for_updates()) { + return; + } + + set_leds(); +} + +void StatusDisplay::publish() +{ + _led_control.timestamp = hrt_absolute_time(); + + _led_control_pub.publish(_led_control); +} + +} /* namespace status */ +} /* namespace events */ diff --git a/src/prometheus_px4/src/modules/events/status_display.h b/src/prometheus_px4/src/modules/events/status_display.h new file mode 100644 index 0000000..f70ce10 --- /dev/null +++ b/src/prometheus_px4/src/modules/events/status_display.h @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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 status_display.h + * Status Display decouples LED and tunes from commander + * + * @author Simone Guscetti + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace events +{ +namespace status +{ + +class StatusDisplay +{ +public: + + StatusDisplay(); + + /** regularily called to handle state updates */ + void process(); + +protected: + /** + * check for topic updates + * @return true if one or more topics got updated + */ + bool check_for_updates(); + + /** + * handle LED logic changes & call publish() + */ + void set_leds(); + + /** publish LED control */ + void publish(); + + // TODO: review if there is a better variant that allocates this in the memory + uORB::SubscriptionData _battery_status_sub{ORB_ID(battery_status)}; + uORB::SubscriptionData _cpu_load_sub{ORB_ID(cpuload)}; + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + uORB::SubscriptionData _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + + struct led_control_s _led_control = {}; + +private: + bool _old_gps_lock_valid = false; + bool _old_home_position_valid = false; + bool _low_battery = false; + bool _critical_battery = false; + int _old_nav_state = -1; + int _old_battery_status_warning = -1; + + uORB::Publication _led_control_pub{ORB_ID(led_control)}; + +}; + +} /* namespace status */ +} /* namespace events */ diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/CMakeLists.txt new file mode 100644 index 0000000..4b7cac1 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/CMakeLists.txt @@ -0,0 +1,131 @@ +############################################################################ +# +# 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. +# +############################################################################ + + +########################################### +# Prepare flight tasks +########################################### + +# add upstream flight tasks (they are being handled differently from the core inside the python script) +list(APPEND flight_tasks_to_add + Orbit +) +# remove possible duplicates +list(REMOVE_DUPLICATES flight_tasks_to_add) + +# remove flight tasks depending on target +if(flight_tasks_to_remove) + list(REMOVE_ITEM flight_tasks_to_add + ${flight_tasks_to_remove} + ) +endif() + +# add core flight tasks to list +list(APPEND flight_tasks_all + AutoFollowMe + AutoLineSmoothVel + Descend + Failsafe + ManualAcceleration + ManualAltitude + ManualAltitudeSmoothVel + ManualPosition + ManualPositionSmoothVel + Transition + ${flight_tasks_to_add} +) + +# set the files to be generated +set(files_to_generate + FlightTasks_generated.hpp + FlightTasks_generated.cpp +) + +# generate files needed for Flight Tasks +set(python_args + -t ${flight_tasks_all} + -i ${CMAKE_CURRENT_SOURCE_DIR}/Templates + -o ${CMAKE_CURRENT_BINARY_DIR} + -f ${files_to_generate} +) + +# add the additional tasks for the python script (if there are any) +if(flight_tasks_to_add) + list(APPEND python_args + -s ${flight_tasks_to_add} + ) +endif() + +# generate the files using the python script and template +add_custom_command( + OUTPUT + ${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp + ${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp + COMMAND ${PYTHON_EXECUTABLE} generate_flight_tasks.py ${python_args} + COMMENT "Generating Flight Tasks" + DEPENDS + Templates/FlightTasks_generated.cpp.em + Templates/FlightTasks_generated.hpp.em + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM +) + +add_compile_options( + -Wno-cast-align +) # TODO: fix and enable + +# add subdirectory containing all tasks +add_subdirectory(tasks) + +px4_add_module( + MODULE modules__flight_mode_manager + MAIN flight_mode_manager + COMPILE_FLAGS + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_BINARY_DIR} + SRCS + FlightModeManager.cpp + FlightModeManager.hpp + + ${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp + ${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp + DEPENDS + px4_work_queue + WeatherVane +) + +# add all flight task dependencies +foreach(task ${flight_tasks_all}) + target_link_libraries(modules__flight_mode_manager PUBLIC FlightTask${task}) +endforeach() diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/FlightModeManager.cpp new file mode 100644 index 0000000..422844b --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -0,0 +1,649 @@ +/**************************************************************************** + * + * Copyright (c) 2020, 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 "FlightModeManager.hpp" + +#include +#include + +using namespace time_literals; + +FlightModeManager::FlightModeManager() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + updateParams(); + + // initialize all flight-tasks + // currently this is required to get all parameters read + for (int i = 0; i < static_cast(FlightTaskIndex::Count); i++) { + _initTask(static_cast(i)); + } + + // disable all tasks + _initTask(FlightTaskIndex::None); +} + +FlightModeManager::~FlightModeManager() +{ + if (_current_task.task) { + _current_task.task->~FlightTask(); + } + + delete _wv_controller; + perf_free(_loop_perf); +} + +bool FlightModeManager::init() +{ + if (!_vehicle_local_position_sub.registerCallback()) { + PX4_ERR("vehicle_local_position callback registration failed!"); + return false; + } + + // limit to every other vehicle_local_position update (50 Hz) + _vehicle_local_position_sub.set_interval_us(20_ms); + _time_stamp_last_loop = hrt_absolute_time(); + return true; +} + +void FlightModeManager::Run() +{ + if (should_exit()) { + _vehicle_local_position_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(¶m_update); + updateParams(); + } + + // generate setpoints on local position changes + vehicle_local_position_s vehicle_local_position; + + if (_vehicle_local_position_sub.update(&vehicle_local_position)) { + const hrt_abstime time_stamp_now = hrt_absolute_time(); + // Guard against too small (< 0.2ms) and too large (> 100ms) dt's. + const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f); + _time_stamp_last_loop = time_stamp_now; + + _home_position_sub.update(); + _vehicle_control_mode_sub.update(); + _vehicle_land_detected_sub.update(); + + if (_vehicle_status_sub.update()) { + if (_vehicle_status_sub.get().is_vtol && (_wv_controller == nullptr)) { + // if vehicle is a VTOL we want to enable weathervane capabilities + _wv_controller = new WeatherVane(); + } + } + + // activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy + // that turns the nose of the vehicle into the wind + if (_wv_controller != nullptr) { + + // in manual mode we just want to use weathervane if position is controlled as well + // in mission, enabling wv is done in flight task + if (_vehicle_control_mode_sub.get().flag_control_manual_enabled) { + if (_vehicle_control_mode_sub.get().flag_control_position_enabled && _wv_controller->weathervane_enabled()) { + _wv_controller->activate(); + + } else { + _wv_controller->deactivate(); + } + } + + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + _vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint); + _wv_controller->update(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z(), vehicle_local_position.heading); + } + + start_flight_task(); + + if (_vehicle_command_sub.updated()) { + handleCommand(); + } + + if (isAnyTaskActive()) { + generateTrajectorySetpoint(dt, vehicle_local_position); + } + + } + + perf_end(_loop_perf); +} + +void FlightModeManager::updateParams() +{ + ModuleParams::updateParams(); + + if (isAnyTaskActive()) { + _current_task.task->handleParameterUpdate(); + } + + if (_wv_controller != nullptr) { + _wv_controller->update_parameters(); + } +} + +void FlightModeManager::start_flight_task() +{ + bool task_failure = false; + bool should_disable_task = true; + int prev_failure_count = _task_failure_count; + + // Do not run any flight task for VTOLs in fixed-wing mode + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + switchTask(FlightTaskIndex::None); + return; + } + + // Only run transition flight task if altitude control is enabled (e.g. in Altitdue, Position, Auto flight mode) + if (_vehicle_status_sub.get().in_transition_mode && _vehicle_control_mode_sub.get().flag_control_altitude_enabled) { + + should_disable_task = false; + FlightTaskError error = switchTask(FlightTaskIndex::Transition); + + if (error != FlightTaskError::NoError) { + if (prev_failure_count == 0) { + PX4_WARN("Transition activation failed with error: %s", errorToString(error)); + } + + task_failure = true; + _task_failure_count++; + + } else { + // we want to be in this mode, reset the failure count + _task_failure_count = 0; + } + + return; + } + + // Auto-follow me + if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { + should_disable_task = false; + FlightTaskError error = switchTask(FlightTaskIndex::AutoFollowMe); + + if (error != FlightTaskError::NoError) { + if (prev_failure_count == 0) { + PX4_WARN("Follow-Me activation failed with error: %s", errorToString(error)); + } + + task_failure = true; + _task_failure_count++; + + } else { + // we want to be in this mode, reset the failure count + _task_failure_count = 0; + } + + } else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) { + // Auto related maneuvers + should_disable_task = false; + FlightTaskError error = FlightTaskError::NoError; + + error = switchTask(FlightTaskIndex::AutoLineSmoothVel); + + if (error != FlightTaskError::NoError) { + if (prev_failure_count == 0) { + PX4_WARN("Auto activation failed with error: %s", errorToString(error)); + } + + task_failure = true; + _task_failure_count++; + + } else { + // we want to be in this mode, reset the failure count + _task_failure_count = 0; + } + + } else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) { + + // Emergency descend + should_disable_task = false; + FlightTaskError error = FlightTaskError::NoError; + + error = switchTask(FlightTaskIndex::Descend); + + if (error != FlightTaskError::NoError) { + if (prev_failure_count == 0) { + PX4_WARN("Descend activation failed with error: %s", errorToString(error)); + } + + task_failure = true; + _task_failure_count++; + + } else { + // we want to be in this mode, reset the failure count + _task_failure_count = 0; + } + + } + + // manual position control + if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) { + should_disable_task = false; + FlightTaskError error = FlightTaskError::NoError; + + switch (_param_mpc_pos_mode.get()) { + case 0: + error = switchTask(FlightTaskIndex::ManualPosition); + break; + + case 3: + error = switchTask(FlightTaskIndex::ManualPositionSmoothVel); + break; + + case 4: + default: + if (_param_mpc_pos_mode.get() != 4) { + PX4_ERR("MPC_POS_MODE %" PRId32 " invalid, resetting", _param_mpc_pos_mode.get()); + _param_mpc_pos_mode.set(4); + _param_mpc_pos_mode.commit(); + } + + error = switchTask(FlightTaskIndex::ManualAcceleration); + break; + } + + if (error != FlightTaskError::NoError) { + if (prev_failure_count == 0) { + PX4_WARN("Position-Ctrl activation failed with error: %s", errorToString(error)); + } + + task_failure = true; + _task_failure_count++; + + } else { + check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_POSCTL); + task_failure = false; + } + } + + // manual altitude control + if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) { + should_disable_task = false; + FlightTaskError error = FlightTaskError::NoError; + + switch (_param_mpc_pos_mode.get()) { + case 0: + error = switchTask(FlightTaskIndex::ManualAltitude); + break; + + case 3: + default: + error = switchTask(FlightTaskIndex::ManualAltitudeSmoothVel); + break; + } + + if (error != FlightTaskError::NoError) { + if (prev_failure_count == 0) { + PX4_WARN("Altitude-Ctrl activation failed with error: %s", errorToString(error)); + } + + task_failure = true; + _task_failure_count++; + + } else { + check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_ALTCTL); + task_failure = false; + } + } + + if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) { + should_disable_task = false; + } + + // check task failure + if (task_failure) { + + // for some reason no flighttask was able to start. + // go into failsafe flighttask + FlightTaskError error = switchTask(FlightTaskIndex::Failsafe); + + if (error != FlightTaskError::NoError) { + // No task was activated. + switchTask(FlightTaskIndex::None); + } + + } else if (should_disable_task) { + switchTask(FlightTaskIndex::None); + } + + _last_vehicle_nav_state = _vehicle_status_sub.get().nav_state; +} + +void FlightModeManager::check_failure(bool task_failure, uint8_t nav_state) +{ + if (!task_failure) { + // we want to be in this mode, reset the failure count + _task_failure_count = 0; + + } else if (_task_failure_count > NUM_FAILURE_TRIES) { + // tell commander to switch mode + PX4_WARN("Previous flight task failed, switching to mode %" PRIu8, nav_state); + send_vehicle_cmd_do(nav_state); + _task_failure_count = 0; // avoid immediate resending of a vehicle command in the next iteration + } +} + +void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state) +{ + vehicle_command_s command{}; + command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; + command.param1 = (float)1; // base mode + command.param3 = (float)0; // sub mode + command.target_system = 1; + command.target_component = 1; + command.source_system = 1; + command.source_component = 1; + command.confirmation = false; + command.from_external = false; + + // set the main mode + switch (nav_state) { + case vehicle_status_s::NAVIGATION_STATE_STAB: + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_STABILIZED; + break; + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; + + default: //vehicle_status_s::NAVIGATION_STATE_POSCTL + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL; + break; + } + + // publish the vehicle command + command.timestamp = hrt_absolute_time(); + _vehicle_command_pub.publish(command); +} + +void FlightModeManager::handleCommand() +{ + // get command + vehicle_command_s command; + + while (_vehicle_command_sub.update(&command)) { + // check what command it is + FlightTaskIndex desired_task = switchVehicleCommand(command.command); + + // ignore all unkown commands + if (desired_task != FlightTaskIndex::None) { + // switch to the commanded task + FlightTaskError switch_result = switchTask(desired_task); + uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + + // if we are in/switched to the desired task + if (switch_result >= FlightTaskError::NoError) { + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + + // if the task is running apply parameters to it and see if it rejects + if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) { + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; + + // if we just switched and parameters are not accepted, go to failsafe + if (switch_result >= FlightTaskError::NoError) { + switchTask(FlightTaskIndex::ManualPosition); + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + } + } + } + + // send back acknowledgment + vehicle_command_ack_s command_ack{}; + command_ack.command = command.command; + command_ack.result = cmd_result; + command_ack.result_param1 = static_cast(switch_result); + command_ack.target_system = command.source_system; + command_ack.target_component = command.source_component; + command_ack.timestamp = hrt_absolute_time(); + _vehicle_command_ack_pub.publish(command_ack); + } + } +} + +void FlightModeManager::generateTrajectorySetpoint(const float dt, + const vehicle_local_position_s &vehicle_local_position) +{ + _current_task.task->setYawHandler(_wv_controller); + + // If the task fails sned out empty NAN setpoints and the controller will emergency failsafe + vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint; + vehicle_constraints_s constraints = FlightTask::empty_constraints; + + if (_current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize()) { + // setpoints and constraints for the position controller from flighttask + setpoint = _current_task.task->getPositionSetpoint(); + constraints = _current_task.task->getConstraints(); + } + + // limit altitude according to land detector + limitAltitude(setpoint, vehicle_local_position); + + if (_takeoff_status_sub.updated()) { + takeoff_status_s takeoff_status; + + if (_takeoff_status_sub.copy(&takeoff_status)) { + _takeoff_state = takeoff_status.takeoff_state; + } + } + + if (_takeoff_state < takeoff_status_s::TAKEOFF_STATE_RAMPUP) { + // reactivate the task which will reset the setpoint to current state + _current_task.task->reActivate(); + } + + + setpoint.timestamp = hrt_absolute_time(); + _trajectory_setpoint_pub.publish(setpoint); + + constraints.timestamp = hrt_absolute_time(); + _vehicle_constraints_pub.publish(constraints); + + // if there's any change in landing gear setpoint publish it + landing_gear_s landing_gear = _current_task.task->getGear(); + + if (landing_gear.landing_gear != _old_landing_gear_position + && landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { + + landing_gear.timestamp = hrt_absolute_time(); + _landing_gear_pub.publish(landing_gear); + } + + _old_landing_gear_position = landing_gear.landing_gear; +} + +void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint, + const vehicle_local_position_s &vehicle_local_position) +{ + if (_vehicle_land_detected_sub.get().alt_max < 0.0f || !_home_position_sub.get().valid_alt + || !vehicle_local_position.z_valid || !vehicle_local_position.v_z_valid) { + // there is no altitude limitation present or the required information not available + return; + } + + // maximum altitude == minimal z-value (NED) + const float min_z = _home_position_sub.get().z + (-_vehicle_land_detected_sub.get().alt_max); + + if (vehicle_local_position.z < min_z) { + // above maximum altitude, only allow downwards flight == positive vz-setpoints (NED) + setpoint.z = min_z; + setpoint.vz = math::max(setpoint.vz, 0.f); + } +} + +FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) +{ + // switch to the running task, nothing to do + if (new_task_index == _current_task.index) { + return FlightTaskError::NoError; + } + + // Save current setpoints for the next FlightTask + vehicle_local_position_setpoint_s last_setpoint = FlightTask::empty_setpoint; + ekf_reset_counters_s last_reset_counters{}; + + if (isAnyTaskActive()) { + last_setpoint = _current_task.task->getPositionSetpoint(); + last_reset_counters = _current_task.task->getResetCounters(); + } + + if (_initTask(new_task_index)) { + // invalid task + return FlightTaskError::InvalidTask; + } + + if (!isAnyTaskActive()) { + // no task running + return FlightTaskError::NoError; + } + + // activation failed + if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) { + _current_task.task->~FlightTask(); + _current_task.task = nullptr; + _current_task.index = FlightTaskIndex::None; + return FlightTaskError::ActivationFailed; + } + + _current_task.task->setResetCounters(last_reset_counters); + + return FlightTaskError::NoError; +} + +FlightTaskError FlightModeManager::switchTask(int new_task_index) +{ + // make sure we are in range of the enumeration before casting + if (static_cast(FlightTaskIndex::None) <= new_task_index && + static_cast(FlightTaskIndex::Count) > new_task_index) { + return switchTask(FlightTaskIndex(new_task_index)); + } + + switchTask(FlightTaskIndex::None); + return FlightTaskError::InvalidTask; +} + +const char *FlightModeManager::errorToString(const FlightTaskError error) +{ + switch (error) { + case FlightTaskError::NoError: return "No Error"; + + case FlightTaskError::InvalidTask: return "Invalid Task "; + + case FlightTaskError::ActivationFailed: return "Activation Failed"; + } + + return "This error is not mapped to a string or is unknown."; +} + +int FlightModeManager::task_spawn(int argc, char *argv[]) +{ + FlightModeManager *instance = new FlightModeManager(); + + 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 FlightModeManager::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FlightModeManager::print_status() +{ + if (isAnyTaskActive()) { + PX4_INFO("Running, active flight task: %" PRIu32, static_cast(_current_task.index)); + + } else { + PX4_INFO("Running, no flight task active"); + } + + perf_print_counter(_loop_perf); + return 0; +} + +int FlightModeManager::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the setpoint generation for all modes. It takes the current mode state of the vehicle as input +and outputs setpoints for controllers. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("flight_mode_manager", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int flight_mode_manager_main(int argc, char *argv[]) +{ + return FlightModeManager::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/FlightModeManager.hpp new file mode 100644 index 0000000..b0f9fdf --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 "FlightTask.hpp" +#include "FlightTasks_generated.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +enum class FlightTaskError : int { + NoError = 0, + InvalidTask = -1, + ActivationFailed = -2 +}; + +class FlightModeManager : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + FlightModeManager(); + ~FlightModeManager() 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); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + bool init(); + +private: + void Run() override; + void updateParams() override; + void start_flight_task(); + void check_failure(bool task_failure, uint8_t nav_state); + void send_vehicle_cmd_do(uint8_t nav_state); + void handleCommand(); + void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position); + void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position); + + /** + * Switch to a specific task (for normal usage) + * @param task index to switch to + * @return 0 on success, <0 on error + */ + FlightTaskError switchTask(FlightTaskIndex new_task_index); + FlightTaskError switchTask(int new_task_index); + + /** + * Call this method to get the description of a task error. + */ + const char *errorToString(const FlightTaskError error); + + /** + * Check if any task is active + * @return true if a task is active, false if not + */ + bool isAnyTaskActive() const { return _current_task.task; } + + // generated + int _initTask(FlightTaskIndex task_index); + FlightTaskIndex switchVehicleCommand(const int command); + + static constexpr int NUM_FAILURE_TRIES = 10; ///< number of tries before switching to a failsafe flight task + + /** + * Union with all existing tasks: we use it to make sure that only the memory of the largest existing + * task is needed, and to avoid using dynamic memory allocations. + */ + TaskUnion _task_union; /**< storage for the currently active task */ + + struct flight_task_t { + FlightTask *task{nullptr}; + FlightTaskIndex index{FlightTaskIndex::None}; + } _current_task{}; + + WeatherVane *_wv_controller{nullptr}; + int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; + uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_UNINITIALIZED}; + int _task_failure_count{0}; + uint8_t _last_vehicle_nav_state{0}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; ///< loop duration performance counter + hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::SubscriptionData _home_position_sub{ORB_ID(home_position)}; + uORB::SubscriptionData _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::SubscriptionData _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; + + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::Publication _vehicle_constraints_pub{ORB_ID(vehicle_constraints)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mpc_pos_mode + ); +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em b/src/prometheus_px4/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em new file mode 100644 index 0000000..8ae9ed4 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 FlightTasks_generated.cpp + * + * Generated file to switch between all required flight tasks + * + * @@author Christoph Tobler + */ + +#include "FlightModeManager.hpp" +#include "FlightTasks_generated.hpp" + +int FlightModeManager::_initTask(FlightTaskIndex task_index) +{ + + // disable the old task if there is any + if (_current_task.task) { + _current_task.task->~FlightTask(); + _current_task.task = nullptr; + _current_task.index = FlightTaskIndex::None; + } + + switch (task_index) { + case FlightTaskIndex::None: + // already disabled task + break; + +@# loop through all requested tasks +@[if tasks]@ +@[for task in tasks]@ +@{ +firstLowercase = lambda s: s[:1].lower() + s[1:] if s else '' +}@ + case FlightTaskIndex::@(task): + _current_task.task = new (&_task_union.@(firstLowercase(task))) FlightTask@(task)(); + break; + +@[end for]@ +@[end if]@ + default: + // invalid task + return 1; + } + + // task construction succeeded + _current_task.index = task_index; + return 0; +} + +FlightTaskIndex FlightModeManager::switchVehicleCommand(const int command) +{ + switch (command) { +@# loop through all additional tasks +@[if tasks_add]@ +@[for task in tasks_add]@ +@{ +upperCase = lambda s: s[:].upper() if s else '' +}@ + case vehicle_command_s::VEHICLE_CMD_DO_@(upperCase(task)) : + return FlightTaskIndex::@(task); + break; + +@[end for]@ +@[end if]@ + // ignore all unkown commands + default : return FlightTaskIndex::None; + } +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/Templates/FlightTasks_generated.hpp.em b/src/prometheus_px4/src/modules/flight_mode_manager/Templates/FlightTasks_generated.hpp.em new file mode 100644 index 0000000..6471d1e --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/Templates/FlightTasks_generated.hpp.em @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 FlightTasks_generated.hpp + * + * Generated Header to list all required flight tasks + * + * @@author Christoph Tobler + */ + + #pragma once + +// include all required headers +#include "FlightModeManager.hpp" +@# loop through all requested tasks +@[if tasks]@ +@[for task in tasks]@ +#include "FlightTask@(task).hpp" +@[end for]@ +@[end if]@ + +enum class FlightTaskIndex : int { + None = -1, +@# loop through all requested tasks +@[if tasks]@ +@[for task in tasks]@ + @(task), +@[end for]@ +@[end if]@ + + Count // number of tasks +}; + +union TaskUnion { + TaskUnion() {} + ~TaskUnion() {} + +@# loop through all requested tasks +@[if tasks]@ +@{ +firstLowercase = lambda s: s[:1].lower() + s[1:] if s else '' +}@ +@[for task in tasks]@ + FlightTask@(task) @(firstLowercase(task)); +@[end for]@ +@[end if]@ +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/flight_mode_manager_params.c b/src/prometheus_px4/src/modules/flight_mode_manager/flight_mode_manager_params.c new file mode 100644 index 0000000..cfe2f76 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/flight_mode_manager_params.c @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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 flight_mode_manager_params.c + */ diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/generate_flight_tasks.py b/src/prometheus_px4/src/modules/flight_mode_manager/generate_flight_tasks.py new file mode 100644 index 0000000..9bbb192 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/generate_flight_tasks.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python + +import em +import os +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument("-t", "--tasks", dest='tasks_all', nargs='+', required=True, help="All tasks to be generated") +parser.add_argument("-s", "--tasks_additional", dest='tasks_add', nargs='+', help="Additional tasks to be generated (on top of the core)") +parser.add_argument("-i", "--input_directory", dest='directory_in', required=True, help="Output directory") +parser.add_argument("-o", "--output_directory", dest='directory_out', required=True, help="Input directory") +parser.add_argument("-f", "--files", dest='gen_files', nargs='+', required=True, help="Files to generate") + +# Parse arguments +args = parser.parse_args() + +for gen_file in args.gen_files: + ofile = args.directory_out + "/" + gen_file + output_file = open(ofile, 'w') + # need to specify the em_globals inside the loop -> em.Error: interpreter globals collision + em_globals = { + "tasks": args.tasks_all, + "tasks_add": args.tasks_add, + } + interpreter = em.Interpreter(output=output_file, globals=em_globals) + interpreter.file(open(args.directory_in + "/" + gen_file + ".em")) + interpreter.shutdown() diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt new file mode 100644 index 0000000..3d0bc84 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskAuto + FlightTaskAuto.cpp +) + +target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility) +target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp new file mode 100644 index 0000000..d2a3d53 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -0,0 +1,519 @@ +/**************************************************************************** + * + * 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 FlightTaskAuto.cpp + */ + +#include "FlightTaskAuto.hpp" +#include +#include + +using namespace matrix; + +static constexpr float SIGMA_NORM = 0.001f; + +FlightTaskAuto::FlightTaskAuto() : + _obstacle_avoidance(this) +{ + +} + +bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTask::activate(last_setpoint); + _position_setpoint = _position; + _velocity_setpoint = _velocity; + _yaw_setpoint = _yaw_sp_prev = _yaw; + _yawspeed_setpoint = 0.0f; + _setDefaultConstraints(); + return ret; +} + +bool FlightTaskAuto::updateInitialize() +{ + bool ret = FlightTask::updateInitialize(); + + _sub_home_position.update(); + _sub_vehicle_status.update(); + _sub_triplet_setpoint.update(); + + // require valid reference and valid target + ret = ret && _evaluateGlobalReference() && _evaluateTriplets(); + // require valid position + ret = ret && PX4_ISFINITE(_position(0)) + && PX4_ISFINITE(_position(1)) + && PX4_ISFINITE(_position(2)) + && PX4_ISFINITE(_velocity(0)) + && PX4_ISFINITE(_velocity(1)) + && PX4_ISFINITE(_velocity(2)); + + return ret; +} + +bool FlightTaskAuto::updateFinalize() +{ + // All the auto FlightTasks have to comply with defined maximum yaw rate + // If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value + // it will see its setpoint constrained here + _limitYawRate(); + _constraints.want_takeoff = _checkTakeoff(); + return true; +} + +void FlightTaskAuto::_limitYawRate() +{ + const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get()); + + _yaw_sp_aligned = true; + + if (PX4_ISFINITE(_yaw_setpoint) && PX4_ISFINITE(_yaw_sp_prev)) { + // Limit the rate of change of the yaw setpoint + const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev); + const float dyaw_max = yawrate_max * _deltatime; + const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max); + const float yaw_setpoint_sat = matrix::wrap_pi(_yaw_sp_prev + dyaw); + + // The yaw setpoint is aligned when it is within tolerance + _yaw_sp_aligned = fabsf(matrix::wrap_pi(_yaw_setpoint - yaw_setpoint_sat)) < math::radians(_param_mis_yaw_err.get()); + + _yaw_setpoint = yaw_setpoint_sat; + _yaw_sp_prev = _yaw_setpoint; + + if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) { + // Create a feedforward + _yawspeed_setpoint = dyaw / _deltatime; + } + } + + if (PX4_ISFINITE(_yawspeed_setpoint)) { + // The yaw setpoint is aligned when its rate is not saturated + _yaw_sp_aligned = _yaw_sp_aligned && (fabsf(_yawspeed_setpoint) < yawrate_max); + + _yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max); + } +} + +bool FlightTaskAuto::_evaluateTriplets() +{ + // TODO: fix the issues mentioned below + // We add here some conditions that are only required because: + // 1. navigator continuously sends triplet during mission due to yaw setpoint. This + // should be removed in the navigator and only updates if the current setpoint actually has changed. + // + // 2. navigator should be responsible to send always three valid setpoints. If there is only one setpoint, + // then previous will be set to current vehicle position and next will be set equal to setpoint. + // + // 3. navigator originally only supports gps guided maneuvers. However, it now also supports some flow-specific features + // such as land and takeoff. The navigator should use for auto takeoff/land with flow the position in xy at the moment the + // takeoff/land was initiated. Until then we do this kind of logic here. + + // Check if triplet is valid. There must be at least a valid altitude. + + if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) { + // Best we can do is to just set all waypoints to current state + _prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position; + _type = WaypointType::loiter; + _yaw_setpoint = _yaw; + _yawspeed_setpoint = NAN; + _target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius; + _updateInternalWaypoints(); + return true; + } + + _type = (WaypointType)_sub_triplet_setpoint.get().current.type; + + // Always update cruise speed since that can change without waypoint changes. + _mc_cruise_speed = _sub_triplet_setpoint.get().current.cruising_speed; + + if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { + // If no speed is planned use the default cruise speed as limit + _mc_cruise_speed = _constraints.speed_xy; + } + + // Ensure planned cruise speed is below the maximum such that the smooth trajectory doesn't get capped + _mc_cruise_speed = math::min(_mc_cruise_speed, _param_mpc_xy_vel_max.get()); + + // Temporary target variable where we save the local reprojection of the latest navigator current triplet. + Vector3f tmp_target; + + if (!PX4_ISFINITE(_sub_triplet_setpoint.get().current.lat) + || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.lon)) { + // No position provided in xy. Lock position + if (!PX4_ISFINITE(_lock_position_xy(0))) { + tmp_target(0) = _lock_position_xy(0) = _position(0); + tmp_target(1) = _lock_position_xy(1) = _position(1); + + } else { + tmp_target(0) = _lock_position_xy(0); + tmp_target(1) = _lock_position_xy(1); + } + + } else { + // reset locked position if current lon and lat are valid + _lock_position_xy.setAll(NAN); + + // Convert from global to local frame. + map_projection_project(&_reference_position, + _sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon, &tmp_target(0), &tmp_target(1)); + } + + tmp_target(2) = -(_sub_triplet_setpoint.get().current.alt - _reference_altitude); + + // Check if anything has changed. We do that by comparing the temporary target + // to the internal _triplet_target. + // TODO This is a hack and it would be much better if the navigator only sends out a waypoints once they have changed. + + bool triplet_update = true; + const bool prev_next_validity_changed = (_prev_was_valid != _sub_triplet_setpoint.get().previous.valid) + || (_next_was_valid != _sub_triplet_setpoint.get().next.valid); + + if (PX4_ISFINITE(_triplet_target(0)) + && PX4_ISFINITE(_triplet_target(1)) + && PX4_ISFINITE(_triplet_target(2)) + && fabsf(_triplet_target(0) - tmp_target(0)) < 0.001f + && fabsf(_triplet_target(1) - tmp_target(1)) < 0.001f + && fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f + && !prev_next_validity_changed) { + // Nothing has changed: just keep old waypoints. + triplet_update = false; + + } else { + _triplet_target = tmp_target; + _target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius; + + if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) { + // Horizontal target is not finite. + _triplet_target(0) = _position(0); + _triplet_target(1) = _position(1); + } + + if (!PX4_ISFINITE(_triplet_target(2))) { + _triplet_target(2) = _position(2); + } + + // If _triplet_target has updated, update also _triplet_prev_wp and _triplet_next_wp. + _prev_prev_wp = _triplet_prev_wp; + + if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) { + map_projection_project(&_reference_position, _sub_triplet_setpoint.get().previous.lat, + _sub_triplet_setpoint.get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1)); + _triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude); + + } else { + _triplet_prev_wp = _position; + } + + _prev_was_valid = _sub_triplet_setpoint.get().previous.valid; + + if (_type == WaypointType::loiter) { + _triplet_next_wp = _triplet_target; + + } else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) { + map_projection_project(&_reference_position, _sub_triplet_setpoint.get().next.lat, + _sub_triplet_setpoint.get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1)); + _triplet_next_wp(2) = -(_sub_triplet_setpoint.get().next.alt - _reference_altitude); + + } else { + _triplet_next_wp = _triplet_target; + } + + _next_was_valid = _sub_triplet_setpoint.get().next.valid; + } + + if (_ext_yaw_handler != nullptr) { + // activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane) + (_param_wv_en.get() && !_sub_triplet_setpoint.get().current.disable_weather_vane) ? _ext_yaw_handler->activate() : + _ext_yaw_handler->deactivate(); + } + + // Calculate the current vehicle state and check if it has updated. + State previous_state = _current_state; + _current_state = _getCurrentState(); + + if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) { + _updateInternalWaypoints(); + _mission_gear = _sub_triplet_setpoint.get().current.landing_gear; + _yaw_lock = false; + } + + if (_param_com_obs_avoid.get() + && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, + _triplet_next_wp, + _sub_triplet_setpoint.get().next.yaw, + _sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN, + _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type); + _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt); + } + + // set heading + if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { + _yaw_setpoint = _yaw; + // use the yawrate setpoint from WV only if not moving lateral (velocity setpoint below half of _param_mpc_xy_cruise) + // otherwise, keep heading constant (as output from WV is not according to wind in this case) + bool vehicle_is_moving_lateral = _velocity_setpoint.xy().longerThan(_param_mpc_xy_cruise.get() / 2.0f); + + if (vehicle_is_moving_lateral) { + _yawspeed_setpoint = 0.0f; + + } else { + _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate(); + } + + + + } else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) { + _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed; + _yaw_setpoint = NAN; + + } else { + if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane) + && _sub_triplet_setpoint.get().current.yaw_valid) { + // Use the yaw computed in Navigator except during takeoff because + // Navigator is not handling the yaw reset properly. + // But: use if from Navigator during takeoff if disable_weather_vane is true, + // because we're then aligning to the transition waypoint. + // TODO: fix in navigator + _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw; + + } else { + _set_heading_from_mode(); + } + + _yawspeed_setpoint = NAN; + } + + return true; +} + +void FlightTaskAuto::_set_heading_from_mode() +{ + + Vector2f v; // Vector that points towards desired location + + switch (_param_mpc_yaw_mode.get()) { + + case 0: // Heading points towards the current waypoint. + case 4: // Same as 0 but yaw first and then go + v = Vector2f(_target) - Vector2f(_position); + break; + + case 1: // Heading points towards home. + if (_sub_home_position.get().valid_lpos) { + v = Vector2f(&_sub_home_position.get().x) - Vector2f(_position); + } + + break; + + case 2: // Heading point away from home. + if (_sub_home_position.get().valid_lpos) { + v = Vector2f(_position) - Vector2f(&_sub_home_position.get().x); + } + + break; + + case 3: // Along trajectory. + // The heading depends on the kind of setpoint generation. This needs to be implemented + // in the subclasses where the velocity setpoints are generated. + v.setAll(NAN); + break; + } + + if (PX4_ISFINITE(v.length())) { + // We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance + // radius, lock yaw to current yaw. + // This prevents excessive yawing. + if (!_yaw_lock) { + if (v.length() < _target_acceptance_radius) { + _yaw_setpoint = _yaw; + _yaw_lock = true; + + } else { + _compute_heading_from_2D_vector(_yaw_setpoint, v); + } + } + + } else { + _yaw_lock = false; + _yaw_setpoint = NAN; + } +} + +bool FlightTaskAuto::_isFinite(const position_setpoint_s &sp) +{ + return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt)); +} + +bool FlightTaskAuto::_evaluateGlobalReference() +{ + // check if reference has changed and update. + // Only update if reference timestamp has changed AND no valid reference altitude + // is available. + // TODO: this needs to be revisited and needs a more clear implementation + if (_sub_vehicle_local_position.get().ref_timestamp == _time_stamp_reference && PX4_ISFINITE(_reference_altitude)) { + // don't need to update anything + return true; + } + + double ref_lat = _sub_vehicle_local_position.get().ref_lat; + double ref_lon = _sub_vehicle_local_position.get().ref_lon; + _reference_altitude = _sub_vehicle_local_position.get().ref_alt; + + if (!_sub_vehicle_local_position.get().z_global) { + // we have no valid global altitude + // set global reference to local reference + _reference_altitude = 0.0f; + } + + if (!_sub_vehicle_local_position.get().xy_global) { + // we have no valid global alt/lat + // set global reference to local reference + ref_lat = 0.0; + ref_lon = 0.0; + } + + // init projection + map_projection_init(&_reference_position, ref_lat, ref_lon); + + // check if everything is still finite + return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon); +} + +void FlightTaskAuto::_setDefaultConstraints() +{ + FlightTask::_setDefaultConstraints(); + + // only adjust limits if the new limit is lower + if (_constraints.speed_xy >= _param_mpc_xy_cruise.get()) { + _constraints.speed_xy = _param_mpc_xy_cruise.get(); + } +} + +Vector2f FlightTaskAuto::_getTargetVelocityXY() +{ + // guard against any bad velocity values + const float vx = _sub_triplet_setpoint.get().current.vx; + const float vy = _sub_triplet_setpoint.get().current.vy; + bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) && + _sub_triplet_setpoint.get().current.velocity_valid; + + if (velocity_valid) { + return Vector2f(vx, vy); + + } else { + // just return zero speed + return Vector2f{}; + } +} + +State FlightTaskAuto::_getCurrentState() +{ + // Calculate the vehicle current state based on the Navigator triplets and the current position. + Vector2f u_prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero(); + Vector2f pos_to_target(_triplet_target - _position); + Vector2f prev_to_pos(_position - _triplet_prev_wp); + // Calculate the closest point to the vehicle position on the line prev_wp - target + _closest_pt = Vector2f(_triplet_prev_wp) + u_prev_to_target * (prev_to_pos * u_prev_to_target); + + State return_state = State::none; + + if (u_prev_to_target * pos_to_target < 0.0f) { + // Target is behind. + return_state = State::target_behind; + + } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _target_acceptance_radius) { + // Current position is more than cruise speed in front of previous setpoint. + return_state = State::previous_infront; + + } else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _target_acceptance_radius) { + // Vehicle is more than cruise speed off track. + return_state = State::offtrack; + + } + + return return_state; +} + +void FlightTaskAuto::_updateInternalWaypoints() +{ + // The internal Waypoints might differ from _triplet_prev_wp, _triplet_target and _triplet_next_wp. + // The cases where it differs: + // 1. The vehicle already passed the target -> go straight to target + // 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint + // 3. The vehicle is more than cruise speed from track -> go straight to closest point on track + switch (_current_state) { + case State::target_behind: + _target = _triplet_target; + _prev_wp = _position; + _next_wp = _triplet_next_wp; + break; + + case State::previous_infront: + _next_wp = _triplet_target; + _target = _triplet_prev_wp; + _prev_wp = _position; + break; + + case State::offtrack: + _next_wp = _triplet_target; + _target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2)); + _prev_wp = _position; + break; + + case State::none: + _target = _triplet_target; + _prev_wp = _triplet_prev_wp; + _next_wp = _triplet_next_wp; + break; + + default: + break; + + } +} + +bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v) +{ + if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) { + v.normalize(); + // To find yaw: take dot product of x = (1,0) and v + // and multiply by the sign given of cross product of x and v. + // Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0) + // Cross product: x(0)*v(1) - v(0)*x(1) = v(1) + heading = sign(v(1)) * wrap_pi(acosf(v(0))); + return true; + } + + // heading unknown and therefore do not change heading + return false; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp new file mode 100644 index 0000000..bccb400 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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 FlightTaskAuto.hpp + * + * Map from global triplet to local quadruple. + */ + +#pragma once + +#include "FlightTask.hpp" +#include +#include +#include +#include +#include +#include + +// TODO: make this switchable in the board config, like a module +#if CONSTRAINED_FLASH +#include +#else +#include +#endif + +/** + * This enum has to agree with position_setpoint_s type definition + * The only reason for not using the struct position_setpoint is because + * of the size + */ +enum class WaypointType : int { + position = position_setpoint_s::SETPOINT_TYPE_POSITION, + velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY, + loiter = position_setpoint_s::SETPOINT_TYPE_LOITER, + takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF, + land = position_setpoint_s::SETPOINT_TYPE_LAND, + idle = position_setpoint_s::SETPOINT_TYPE_IDLE, + follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET, +}; + +enum class State { + offtrack, /**< Vehicle is more than cruise speed away from track */ + target_behind, /**< Vehicle is in front of target. */ + previous_infront, /**< Vehilce is behind previous waypoint.*/ + none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ +}; + +class FlightTaskAuto : public FlightTask +{ +public: + FlightTaskAuto(); + + virtual ~FlightTaskAuto() = default; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool updateInitialize() override; + bool updateFinalize() override; + + /** + * Sets an external yaw handler which can be used to implement a different yaw control strategy. + */ + void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} + +protected: + void _setDefaultConstraints() override; + matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ + void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ + bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */ + + matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */ + matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */ + bool _prev_was_valid{false}; + matrix::Vector3f _target{}; /**< Target waypoint (local frame).*/ + matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */ + bool _next_was_valid{false}; + float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */ + WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */ + + uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; + uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; + + State _current_state{State::none}; + float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */ + int _mission_gear{landing_gear_s::GEAR_KEEP}; + + float _yaw_sp_prev{NAN}; + bool _yaw_sp_aligned{false}; + + ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */ + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_xy_cruise, + (ParamFloat) + _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated + (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, + (ParamInt) _param_com_obs_avoid, // obstacle avoidance active + (ParamFloat) _param_mpc_yawrauto_max, + (ParamFloat) _param_mis_yaw_err, // yaw-error threshold + (ParamBool) _param_wv_en // enable/disable weather vane (VTOL) + ); + +private: + matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */ + bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */ + + uORB::SubscriptionData _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)}; + + matrix::Vector3f + _triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */ + matrix::Vector3f + _triplet_prev_wp; /**< previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.*/ + matrix::Vector3f + _triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/ + matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */ + + map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */ + float _reference_altitude{NAN}; /**< Altitude relative to ground. */ + hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */ + + WeatherVane *_ext_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + + + void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */ + bool _evaluateTriplets(); /**< Checks and sets triplets. */ + bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */ + bool _evaluateGlobalReference(); /**< Check is global reference is available. */ + State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */ + void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */ +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt new file mode 100644 index 0000000..2e757dc --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskAutoFollowMe + FlightTaskAutoFollowMe.cpp +) + +target_link_libraries(FlightTaskAutoFollowMe PUBLIC FlightTaskAuto) +target_include_directories(FlightTaskAutoFollowMe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp new file mode 100644 index 0000000..dda7374 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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 FlightTaskAutoFollowMe.cpp + */ + +#include "FlightTaskAutoFollowMe.hpp" +#include + +using namespace matrix; + +bool FlightTaskAutoFollowMe::update() +{ + bool ret = FlightTaskAuto::update(); + _position_setpoint = _target; + matrix::Vector2f vel_sp = _getTargetVelocityXY(); + _velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f); + return ret; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp new file mode 100644 index 0000000..733c8d3 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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 FlightTaskAutoFollowMe.hpp + * + * Flight task for autonomous, gps driven follow-me mode. + */ + +#pragma once + +#include "FlightTaskAuto.hpp" + +class FlightTaskAutoFollowMe : public FlightTaskAuto +{ +public: + FlightTaskAutoFollowMe() = default; + virtual ~FlightTaskAutoFollowMe() = default; + bool update() override; +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt new file mode 100644 index 0000000..4e22acd --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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(FlightTaskAutoLineSmoothVel + FlightTaskAutoLineSmoothVel.cpp +) + +target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper FlightTaskUtility) +target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + + +px4_add_unit_gtest(SRC TrajectoryConstraintsTest.cpp) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp new file mode 100644 index 0000000..b9e79d5 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -0,0 +1,490 @@ +/**************************************************************************** + * + * 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 FlightAutoLine.cpp + */ + +#include "FlightTaskAutoLineSmoothVel.hpp" + +#include "TrajectoryConstraints.hpp" + +using namespace matrix; + +bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTaskAutoMapper::activate(last_setpoint); + + Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz}; + Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z}; + Vector3f accel_prev{last_setpoint.acceleration}; + + for (int i = 0; i < 3; i++) { + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } + + // No acceleration estimate available, set to zero if the setpoint is NAN + if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } + } + + for (int i = 0; i < 3; ++i) { + _trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i)); + } + + _yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; + _updateTrajConstraints(); + + return ret; +} + +void FlightTaskAutoLineSmoothVel::reActivate() +{ + FlightTaskAutoMapper::reActivate(); + + // On ground, reset acceleration and velocity to zero + for (int i = 0; i < 2; ++i) { + _trajectory[i].reset(0.f, 0.f, _position(i)); + } + + _trajectory[2].reset(0.f, 0.7f, _position(2)); +} + +/** + * EKF reset handling functions + * Those functions are called by the base FlightTask in + * case of an EKF reset event + */ +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY() +{ + _trajectory[0].setCurrentPosition(_position(0)); + _trajectory[1].setCurrentPosition(_position(1)); +} + +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY() +{ + _trajectory[0].setCurrentVelocity(_velocity(0)); + _trajectory[1].setCurrentVelocity(_velocity(1)); +} + +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ() +{ + _trajectory[2].setCurrentPosition(_position(2)); +} + +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ() +{ + _trajectory[2].setCurrentVelocity(_velocity(2)); +} + +void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi) +{ + _yaw_sp_prev += delta_psi; +} + +void FlightTaskAutoLineSmoothVel::_generateSetpoints() +{ + _updateTurningCheck(); + _prepareSetpoints(); + _generateTrajectory(); + + if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) { + // no valid heading -> generate heading in this flight task + _generateHeading(); + } +} + +void FlightTaskAutoLineSmoothVel::_updateTurningCheck() +{ + const Vector2f vel_traj(_trajectory[0].getCurrentVelocity(), + _trajectory[1].getCurrentVelocity()); + const Vector2f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition()); + const Vector2f target_xy(_target); + const Vector2f u_vel_traj = vel_traj.unit_or_zero(); + const Vector2f pos_to_target = Vector2f(target_xy - pos_traj); + const float cos_align = u_vel_traj * pos_to_target.unit_or_zero(); + + // The vehicle is turning if the angle between the velocity vector + // and the direction to the target is greater than 10 degrees, the + // velocity is large enough and the drone isn't in the acceptance + // radius of the last WP. + _is_turning = vel_traj.longerThan(0.2f) + && cos_align < 0.98f + && pos_to_target.longerThan(_target_acceptance_radius); +} + +void FlightTaskAutoLineSmoothVel::_generateHeading() +{ + // Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint + if (!_generateHeadingAlongTraj()) { + _yaw_setpoint = _yaw_sp_prev; + } +} + +bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj() +{ + bool res = false; + Vector2f vel_sp_xy(_velocity_setpoint); + Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position); + + if ((vel_sp_xy.length() > .1f) && + (traj_to_target.length() > 2.f)) { + // Generate heading from velocity vector, only if it is long enough + // and if the drone is far enough from the target + _compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy); + res = true; + } + + return res; +} + +/* Constrain some value vith a constrain depending on the sign of the constraint + * Example: - if the constrain is -5, the value will be constrained between -5 and 0 + * - if the constrain is 5, the value will be constrained between 0 and 5 + */ +float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint) +{ + const float min = (constraint < FLT_EPSILON) ? constraint : 0.f; + const float max = (constraint > FLT_EPSILON) ? constraint : 0.f; + + return math::constrain(val, min, max); +} + +float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max) +{ + return sign(val) * math::min(fabsf(val), fabsf(max)); +} + +float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const +{ + Vector3f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + + math::trajectory::VehicleDynamicLimits config; + config.z_accept_rad = _param_nav_mc_alt_rad.get(); + config.xy_accept_rad = _target_acceptance_radius; + config.max_acc_xy = _trajectory[0].getMaxAccel(); + config.max_jerk = _trajectory[0].getMaxJerk(); + config.max_speed_xy = _mc_cruise_speed; + config.max_acc_xy_radius_scale = _param_mpc_xy_traj_p.get(); + + // constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source + // (eg. Obstacle Avoidance) + + Vector3f waypoints[3] = {pos_traj, _target, _next_wp}; + + if (isTargetModified()) { + waypoints[2] = waypoints[1] = _position_setpoint; + } + + float max_xy_speed = math::trajectory::computeXYSpeedFromWaypoints<3>(waypoints, config); + + return max_xy_speed; +} + +float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const +{ + Vector3f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + float z_setpoint = _target(2); + + // constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source + // (eg. Obstacle Avoidance) + bool z_valid = PX4_ISFINITE(_position_setpoint(2)); + bool z_modified = z_valid && fabsf((_target - _position_setpoint)(2)) > FLT_EPSILON; + + if (z_modified) { + z_setpoint = _position_setpoint(2); + } + + const float distance_start_target = fabs(z_setpoint - pos_traj(2)); + const float arrival_z_speed = 0.f; + + float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( + _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), + distance_start_target, arrival_z_speed)); + + return max_speed; +} + +Vector3f FlightTaskAutoLineSmoothVel::getCrossingPoint() const +{ + Vector3f pos_crossing_point{}; + + if (isTargetModified()) { + // Strictly follow the modified setpoint + pos_crossing_point = _position_setpoint; + + } else { + if (_is_turning) { + // Get the crossing point using L1-style guidance + pos_crossing_point.xy() = getL1Point(); + pos_crossing_point(2) = _target(2); + + } else { + pos_crossing_point = _target; + } + } + + return pos_crossing_point; +} + +bool FlightTaskAutoLineSmoothVel::isTargetModified() const +{ + const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); + const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); + const bool z_modified = z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; + + return xy_modified || z_modified; +} + +Vector2f FlightTaskAutoLineSmoothVel::getL1Point() const +{ + const Vector2f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition()); + const Vector2f target_xy(_target); + const Vector2f u_prev_to_target = Vector2f(target_xy - Vector2f(_prev_wp)).unit_or_zero(); + const Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp)); + const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target)); + const Vector2f closest_pt = Vector2f(_prev_wp) + prev_to_closest; + + // Compute along-track error using L1 distance and cross-track error + const float crosstrack_error = Vector2f(closest_pt - pos_traj).length(); + + const float l1 = math::max(_target_acceptance_radius, 5.f); + float alongtrack_error = 0.f; + + // Protect against sqrt of a negative number + if (l1 > crosstrack_error) { + alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error); + } + + // Position of the point on the line where L1 intersect the line between the two waypoints + const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target; + + return l1_point; +} + +void FlightTaskAutoLineSmoothVel::_prepareSetpoints() +{ + // Interface: A valid position setpoint generates a velocity target using conservative motion constraints. + // If a velocity is specified, that is used as a feedforward to track the position setpoint + // (ie. it assumes the position setpoint is moving at the specified velocity) + // If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here. + + _want_takeoff = false; + + if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) { + // Wait for the yaw setpoint to be aligned + _velocity_setpoint.setAll(0.f); + return; + } + + const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1)); + const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2)); + + if (xy_pos_setpoint_valid && z_pos_setpoint_valid) { + // Use 3D position setpoint to generate a 3D velocity setpoint + Vector3f pos_traj(_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + + const Vector3f u_pos_traj_to_dest((getCrossingPoint() - pos_traj).unit_or_zero()); + + float xy_speed = _getMaxXYSpeed(); + const float z_speed = _getMaxZSpeed(); + + if (_is_turning) { + // Lock speed during turn + xy_speed = math::min(_max_speed_prev, xy_speed); + + } else { + _max_speed_prev = xy_speed; + } + + Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed); + math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f); + math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f); + + for (int i = 0; i < 3; i++) { + // If available, use the existing velocity as a feedforward, otherwise replace it + if (PX4_ISFINITE(_velocity_setpoint(i))) { + _velocity_setpoint(i) += vel_sp_constrained(i); + + } else { + _velocity_setpoint(i) = vel_sp_constrained(i); + } + } + } + + else if (xy_pos_setpoint_valid) { + // Use 2D position setpoint to generate a 2D velocity setpoint + + // Get various path specific vectors + Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition()); + Vector2f pos_traj_to_dest_xy = Vector2f(getCrossingPoint()) - pos_traj; + Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero()); + + float xy_speed = _getMaxXYSpeed(); + + if (_is_turning) { + // Lock speed during turn + xy_speed = math::min(_max_speed_prev, xy_speed); + + } else { + _max_speed_prev = xy_speed; + } + + Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * xy_speed; + + for (int i = 0; i < 2; i++) { + // If available, use the existing velocity as a feedforward, otherwise replace it + if (PX4_ISFINITE(_velocity_setpoint(i))) { + _velocity_setpoint(i) += vel_sp_constrained_xy(i); + + } else { + _velocity_setpoint(i) = vel_sp_constrained_xy(i); + } + } + } + + else if (z_pos_setpoint_valid) { + // Use Z position setpoint to generate a Z velocity setpoint + + const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition()); + const float vel_sp_z = z_dir * _getMaxZSpeed(); + + // If available, use the existing velocity as a feedforward, otherwise replace it + if (PX4_ISFINITE(_velocity_setpoint(2))) { + _velocity_setpoint(2) += vel_sp_z; + + } else { + _velocity_setpoint(2) = vel_sp_z; + } + } + + _want_takeoff = _velocity_setpoint(2) < -0.3f; +} + +void FlightTaskAutoLineSmoothVel::_updateTrajConstraints() +{ + // Update the constraints of the trajectories + _trajectory[0].setMaxAccel(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading + _trajectory[1].setMaxAccel(_param_mpc_acc_hor.get()); + _trajectory[0].setMaxVel(_param_mpc_xy_vel_max.get()); + _trajectory[1].setMaxVel(_param_mpc_xy_vel_max.get()); + _trajectory[0].setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading + _trajectory[1].setMaxJerk(_param_mpc_jerk_auto.get()); + _trajectory[2].setMaxJerk(_param_mpc_jerk_auto.get()); + + if (_velocity_setpoint(2) < 0.f) { // up + float z_accel_constraint = _param_mpc_acc_up_max.get(); + float z_vel_constraint = _param_mpc_z_vel_max_up.get(); + + // The constraints are broken because they are used as hard limits by the position controller, so put this here + // until the constraints don't do things like cause controller integrators to saturate. Once the controller + // doesn't use z speed constraints, this can go in AutoMapper::_prepareTakeoffSetpoints(). Accel limit is to + // emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint. + if (_type == WaypointType::takeoff && _dist_to_ground < _param_mpc_land_alt1.get()) { + z_vel_constraint = _param_mpc_tko_speed.get(); + z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get()); + + // Keep the altitude setpoint at the current altitude + // to avoid having it going down into the ground during + // the initial ramp as the velocity does not start at 0 + _trajectory[2].setCurrentPosition(_position(2)); + } + + _trajectory[2].setMaxVel(z_vel_constraint); + _trajectory[2].setMaxAccel(z_accel_constraint); + + } else { // down + _trajectory[2].setMaxAccel(_param_mpc_acc_down_max.get()); + _trajectory[2].setMaxVel(_param_mpc_z_vel_max_dn.get()); + } +} + +void FlightTaskAutoLineSmoothVel::_generateTrajectory() +{ + if (!PX4_ISFINITE(_velocity_setpoint(0)) || !PX4_ISFINITE(_velocity_setpoint(1)) + || !PX4_ISFINITE(_velocity_setpoint(2))) { + return; + } + + /* Slow down the trajectory by decreasing the integration time based on the position error. + * This is only performed when the drone is behind the trajectory + */ + Vector2f position_trajectory_xy(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition()); + Vector2f position_xy(_position); + Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity()); + Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy); + float position_error = drone_to_trajectory_xy.length(); + + float time_stretch = 1.f - math::constrain(position_error / _param_mpc_xy_err_max.get(), 0.f, 1.f); + + // Don't stretch time if the drone is ahead of the position setpoint + if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) { + time_stretch = 1.f; + } + + Vector3f jerk_sp_smooth; + Vector3f accel_sp_smooth; + Vector3f vel_sp_smooth; + Vector3f pos_sp_smooth; + + for (int i = 0; i < 3; ++i) { + _trajectory[i].updateTraj(_deltatime, time_stretch); + jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk(); + accel_sp_smooth(i) = _trajectory[i].getCurrentAcceleration(); + vel_sp_smooth(i) = _trajectory[i].getCurrentVelocity(); + pos_sp_smooth(i) = _trajectory[i].getCurrentPosition(); + } + + _updateTrajConstraints(); + + for (int i = 0; i < 3; ++i) { + _trajectory[i].updateDurations(_velocity_setpoint(i)); + } + + VelocitySmoothing::timeSynchronization(_trajectory, 3); + + _jerk_setpoint = jerk_sp_smooth; + _acceleration_setpoint = accel_sp_smooth; + _velocity_setpoint = vel_sp_smooth; + _position_setpoint = pos_sp_smooth; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp new file mode 100644 index 0000000..1a6ceb8 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 FlightTaskAutoLineSmoothVel.hpp + * + * Flight task for autonomous, gps driven mode. The vehicle flies + * along a straight line in between waypoints. + */ + +#pragma once + +#include "FlightTaskAutoMapper.hpp" +#include + +class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper +{ +public: + FlightTaskAutoLineSmoothVel() = default; + virtual ~FlightTaskAutoLineSmoothVel() = default; + + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + void reActivate() override; + +protected: + + /** Reset position or velocity setpoints in case of EKF reset event */ + void _ekfResetHandlerPositionXY() override; + void _ekfResetHandlerVelocityXY() override; + void _ekfResetHandlerPositionZ() override; + void _ekfResetHandlerVelocityZ() override; + void _ekfResetHandlerHeading(float delta_psi) override; + + void _generateSetpoints() override; /**< Generate setpoints along line. */ + void _generateHeading(); + void _updateTurningCheck(); + bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ + + static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */ + + static float _constrainAbs(float val, float max); /** Constrain the value -max <= val <= max */ + + float _getMaxXYSpeed() const; + float _getMaxZSpeed() const; + + matrix::Vector3f getCrossingPoint() const; + bool isTargetModified() const; + matrix::Vector2f getL1Point() const; + + float _max_speed_prev{}; + bool _is_turning{false}; + + void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */ + void _updateTrajConstraints(); + void _generateTrajectory(); + + /** determines when to trigger a takeoff (ignored in flight) */ + bool _checkTakeoff() override { return _want_takeoff; }; + bool _want_takeoff{false}; + + VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper, + (ParamFloat) _param_mis_yaw_err, // yaw-error threshold + (ParamFloat) _param_mpc_acc_hor, // acceleration in flight + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_xy_traj_p, + (ParamFloat) _param_mpc_xy_err_max + ); +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp new file mode 100644 index 0000000..5153068 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include + +namespace math +{ +namespace trajectory +{ +using matrix::Vector3f; +using matrix::Vector2f; + +struct VehicleDynamicLimits { + float z_accept_rad; + float xy_accept_rad; + + float max_acc_xy; + float max_jerk; + + float max_speed_xy; + + // TODO: remove this + float max_acc_xy_radius_scale; +}; + +/* + * Compute the maximum allowed speed at the waypoint assuming that we want to + * connect the two lines (current-target and target-next) + * with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius + * The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius. + * This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that + * the real acceptance radius is smaller. + * + */ +inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target, + const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config) +{ + const float distance_target_next = (target - next_target).xy().norm(); + + const bool target_next_different = distance_target_next > 0.001f; + const bool waypoint_overlap = distance_target_next < config.xy_accept_rad; + const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad; + const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad; + + float speed_at_target = 0.0f; + + if (target_next_different && + !waypoint_overlap && + has_reached_altitude && + altitude_stays_same + ) { + const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot( + Vector2f((target - next_target).xy()).unit_or_zero())); + const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON); + float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy; + float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad); + speed_at_target = min(min(max_speed_in_turn, exit_speed), config.max_speed_xy); + } + + float start_to_target = (start_position - target).xy().norm(); + float max_speed = computeMaxSpeedFromDistance(config.max_jerk, config.max_acc_xy, start_to_target, speed_at_target); + + return min(config.max_speed_xy, max_speed); +} + +/* + * This function computes the maximum speed XY that can be travelled, given a set of waypoints and vehicle dynamics + * + * The first waypoint should be the starting location, and the later waypoints the desired points to be followed. + * + * @param waypoints the list of waypoints to be followed, the first of which should be the starting location + * @param config the vehicle dynamic limits + * + * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits + */ +template +float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config) +{ + static_assert(N >= 2, "Need at least 2 points to compute speed"); + + float max_speed = 0.f; + + for (size_t j = 0; j < N - 1; j++) { + size_t i = N - 2 - j; + max_speed = computeStartXYSpeedFromWaypoints(waypoints[i], + waypoints[i + 1], + waypoints[min(i + 2, N - 1)], + max_speed, config); + } + + return max_speed; +} + +/* + * Constrain the 3D vector given a maximum XY norm + * If the XY norm of the 3D vector is larger than the maximum norm, the whole vector + * is scaled down to respect the constraint. + * If the maximum norm is small (defined by the "accuracy" parameter), + * only the XY components are scaled down to avoid affecting + * Z in case of numerical issues + */ +inline void clampToXYNorm(Vector3f &target, float max_xy_norm, float accuracy = FLT_EPSILON) +{ + const float xynorm = target.xy().norm(); + const float scale_factor = (xynorm > FLT_EPSILON) + ? max_xy_norm / xynorm + : 1.f; + + if (scale_factor < 1.f) { + if (max_xy_norm < accuracy && xynorm < accuracy) { + target.xy() = Vector2f(target) * scale_factor; + + } else { + target *= scale_factor; + } + } +} + +/* + * Constrain the 3D vector given a maximum Z norm + * If the Z component of the 3D vector is larger than the maximum norm, the whole vector + * is scaled down to respect the constraint. + * If the maximum norm is small (defined by the "accuracy parameter), + * only the Z component is scaled down to avoid affecting + * XY in case of numerical issues + */ +inline void clampToZNorm(Vector3f &target, float max_z_norm, float accuracy = FLT_EPSILON) +{ + const float znorm = fabs(target(2)); + const float scale_factor = (znorm > FLT_EPSILON) + ? max_z_norm / znorm + : 1.f; + + if (scale_factor < 1.f) { + if (max_z_norm < accuracy && znorm < accuracy) { + target(2) *= scale_factor; + + } else { + target *= scale_factor; + } + } +} + +} +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp new file mode 100644 index 0000000..f59302d --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp @@ -0,0 +1,364 @@ +#include + +#include "TrajectoryConstraints.hpp" + +using namespace matrix; +using namespace math::trajectory; + +class TrajectoryConstraintsTest : public ::testing::Test +{ +public: + VehicleDynamicLimits config; + + Vector3f vehicle_location; + Vector3f target; + Vector3f next_target; + + float final_speed = 0; + + void SetUp() override + { + config.z_accept_rad = 1.f; + config.xy_accept_rad = 0.99f; + + config.max_acc_xy = 3.f; + config.max_jerk = 10.f; + + config.max_speed_xy = 10.f; + + config.max_acc_xy_radius_scale = 0.8f; + + /* + * (20,20) + * Next target + * + * ^ + * | + * + * (10,10) (20,10) + * Vehicle -> Target + * + */ + vehicle_location = Vector3f(10, 10, 5); + target = Vector3f(20, 10, 5); + next_target = Vector3f(20, 20, 5); + } +}; + +TEST_F(TrajectoryConstraintsTest, testStraight) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.5f * (next_target - vehicle_location); + + // WHEN: we get the speed for straight line travel + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed directly to the end point + Vector3f direct_points[2] = {vehicle_location, next_target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + +TEST_F(TrajectoryConstraintsTest, testStraightNaN) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.5f * (next_target - vehicle_location); + next_target(0) = NAN; + next_target(1) = NAN; + + // WHEN: we get the speed for points which are NaN afterwards + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed to the closer point + Vector3f direct_points[2] = {vehicle_location, target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + +TEST_F(TrajectoryConstraintsTest, testStraightLowJerkClose) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.05f * (next_target - vehicle_location); + config.max_jerk = 8.f; + + // WHEN: we get the speed for straight line travel + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed directly to the end point + Vector3f direct_points[2] = {vehicle_location, next_target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + +TEST_F(TrajectoryConstraintsTest, testStraightMidClose) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.05f * (next_target - vehicle_location); + + // WHEN: we get the speed for straight line travel + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed directly to the end point + Vector3f direct_points[2] = {vehicle_location, next_target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + +TEST_F(TrajectoryConstraintsTest, testStraightMidFar) +{ + // GIVEN: 3 waypoints in straight line + next_target = target + 2.f * (target - vehicle_location); + target = vehicle_location + 0.95f * (next_target - vehicle_location); + + // WHEN: we get the speed for straight line travel + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be the same as speed directly to the end point + Vector3f direct_points[2] = {vehicle_location, next_target}; + float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config); + + EXPECT_FLOAT_EQ(through_speed, direct_speed); +} + + +TEST_F(TrajectoryConstraintsTest, test90Angle) +{ + // GIVEN: 3 waypoints in 90 degree angle + EXPECT_FLOAT_EQ(0.f, (vehicle_location - target).dot(target - next_target)); + + // WHEN: we get the speed for travel around the path + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be slightly faster than stopping at the intermediate point + Vector3f stop_points[2] = {vehicle_location, target}; + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + + EXPECT_GT(through_speed, stop_speed); //faster + EXPECT_LT(through_speed, stop_speed * 1.03f); // but less than 3% faster +} + +TEST_F(TrajectoryConstraintsTest, test45Angle) +{ + // GIVEN: 3 waypoints in 45 degree angle + next_target = Vector3f(25, 15, 5); + + // WHEN: we get the speed for travel around the path + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be slightly faster than stopping at the intermediate point + Vector3f stop_points[2] = {vehicle_location, target}; + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + + EXPECT_GT(through_speed, stop_speed * 1.03f); // more than 3% faster + EXPECT_LT(through_speed, stop_speed * 1.06f); // but less than 6% faster +} + +TEST_F(TrajectoryConstraintsTest, test10Angle) +{ + // GIVEN: 3 waypoints in 10 degree angle + next_target = Vector3f(30, 11.7, 5); + + // WHEN: we get the speed for travel around the path + Vector3f waypoints[3] = {vehicle_location, target, next_target}; + float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config); + + // THEN: it should be slightly faster than stopping at the intermediate point + Vector3f stop_points[2] = {vehicle_location, target}; + float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config); + + EXPECT_GT(through_speed, stop_speed * 1.25f); // more than 25% faster + EXPECT_LT(through_speed, stop_speed * 1.3f); // but less than 30% faster +} + +TEST_F(TrajectoryConstraintsTest, test10AngleFarNext) +{ + // GIVEN: 3 waypoints in 10 degree angle, but next waypoint is far + next_target = 2.f * (Vector3f(30, 11.7, 5) - target) + target; + + // WHEN: we get the speed for travel around the path + Vector3f far_waypoints[3] = {vehicle_location, target, next_target}; + float far_speed = computeXYSpeedFromWaypoints<3>(far_waypoints, config); + + // THEN: it should be the same speed as a closer next waypoint at the same angle, since the bottleneck is the turn + next_target = Vector3f(30, 11.7, 5); + Vector3f close_waypoints[3] = {vehicle_location, target, next_target}; + float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config); + + EXPECT_FLOAT_EQ(far_speed, close_speed); +} + +TEST_F(TrajectoryConstraintsTest, test10AngleCloseNext) +{ + // GIVEN: 3 waypoints in right angle, but next waypoint is far + next_target = .2f * (Vector3f(30, 11.7, 5) - target) + target; + + // WHEN: we get the speed for travel around the path + Vector3f close_waypoints[3] = {vehicle_location, target, next_target}; + float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config); + + // THEN: it should be slower than a further next waypoint at the same angle, since the bottleneck is the distance + next_target = Vector3f(30, 11.7, 5); + Vector3f normal_waypoints[3] = {vehicle_location, target, next_target}; + float normal_speed = computeXYSpeedFromWaypoints<3>(normal_waypoints, config); + + EXPECT_LT(close_speed, normal_speed); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormNoEffectLarge) +{ + // GIVEN: a short vector + Vector3f vec(1, 2, 3); + + // WHEN: we clamp it on XY with a long cutoff + clampToXYNorm(vec, 1000.f); + + // THEN: it shouldn't change + EXPECT_EQ(vec, Vector3f(1, 2, 3)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormNoEffect) +{ + // GIVEN: a short vector + Vector3f vec(1, 2, 3); + + // WHEN: we clamp it on XY with a long cutoff + clampToZNorm(vec, 1000.f); + + // THEN: it shouldn't change + EXPECT_EQ(vec, Vector3f(1, 2, 3)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormNoEffectExact) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 1); + + // WHEN: we clamp it on XY with exact cutoff + clampToXYNorm(vec, 5.f); + + // THEN: it shouldn't change + EXPECT_EQ(vec, Vector3f(3, 4, 1)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormNoEffectExact) +{ + // GIVEN: a vector + Vector3f vec(3, 4, -1); + + // WHEN: we clamp it on Z with exact cutoff + clampToZNorm(vec, 1.f); + + // THEN: it shouldn't change + EXPECT_EQ(vec, Vector3f(3, 4, -1)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormHalf) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 1); + + // WHEN: we clamp it on XY with half hypot length + clampToXYNorm(vec, 2.5f); + + // THEN: it should be half length + EXPECT_TRUE(vec == Vector3f(1.5f, 2.f, 0.5f)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormHalf) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 10); + + // WHEN: we clamp it on Z with half length + clampToZNorm(vec, 5.f); + + // THEN: it should be half length + EXPECT_TRUE(vec == Vector3f(1.5f, 2.f, 5.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormZero) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 1); + + // WHEN: we clamp it on XY with half hypot length + clampToXYNorm(vec, 0.f); + + // THEN: it should be 0 + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormZero) +{ + // GIVEN: a vector + Vector3f vec(3, 4, 1); + + // WHEN: we clamp it on Z with half hypot length + clampToZNorm(vec, 0.f); + + // THEN: it should be 0 + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormVecZero) +{ + // GIVEN: a vector + Vector3f vec(0, 0, 0); + + // WHEN: we clamp it on XY + clampToXYNorm(vec, 1.f); + + // THEN: it should be 0 still + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormVecZero) +{ + // GIVEN: a vector + Vector3f vec(0, 0, 0); + + // WHEN: we clamp it on Z + clampToZNorm(vec, 1.f); + + // THEN: it should be 0 still + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToXYNormVecZeroToZero) +{ + // GIVEN: a vector + Vector3f vec(0, 0, 0); + + // WHEN: we clamp it on XY + clampToXYNorm(vec, 0.f); + + // THEN: it should be 0 still + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} + +TEST(TrajectoryConstraintsClamp, clampToZNormVecZeroToZero) +{ + // GIVEN: a vector + Vector3f vec(0, 0, 0); + + // WHEN: we clamp it on XY + clampToZNorm(vec, 0.f); + + // THEN: it should be 0 still + EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f)); +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/CMakeLists.txt new file mode 100644 index 0000000..28d05f0 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskAutoMapper + FlightTaskAutoMapper.cpp +) + +target_link_libraries(FlightTaskAutoMapper PUBLIC FlightTaskAuto) +target_include_directories(FlightTaskAutoMapper PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp new file mode 100644 index 0000000..0699af4 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * 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 FlightAutoMapper.cpp + */ + +#include "FlightTaskAutoMapper.hpp" +#include + +using namespace matrix; + +FlightTaskAutoMapper::FlightTaskAutoMapper() : + _sticks(this) +{} + +bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTaskAuto::activate(last_setpoint); + _reset(); + return ret; +} + +bool FlightTaskAutoMapper::update() +{ + bool ret = FlightTaskAuto::update(); + // always reset constraints because they might change depending on the type + _setDefaultConstraints(); + + // The only time a thrust set-point is sent out is during + // idle. Hence, reset thrust set-point to NAN in case the + // vehicle exits idle. + + if (_type_previous == WaypointType::idle) { + _acceleration_setpoint.setNaN(); + } + + // during mission and reposition, raise the landing gears but only + // if altitude is high enough + if (_highEnoughForLandingGear()) { + _gear.landing_gear = landing_gear_s::GEAR_UP; + } + + switch (_type) { + case WaypointType::idle: + _prepareIdleSetpoints(); + break; + + case WaypointType::land: + _prepareLandSetpoints(); + break; + + case WaypointType::loiter: + + /* fallthrought */ + case WaypointType::position: + _preparePositionSetpoints(); + break; + + case WaypointType::takeoff: + _prepareTakeoffSetpoints(); + break; + + case WaypointType::velocity: + _prepareVelocitySetpoints(); + break; + + default: + _preparePositionSetpoints(); + break; + } + + if (_param_com_obs_avoid.get()) { + _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); + _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, + _yawspeed_setpoint); + } + + + _generateSetpoints(); + + // update previous type + _type_previous = _type; + + return ret; +} + +void FlightTaskAutoMapper::_reset() +{ + // Set setpoints equal current state. + _velocity_setpoint = _velocity; + _position_setpoint = _position; +} + +void FlightTaskAutoMapper::_prepareIdleSetpoints() +{ + // Send zero thrust setpoint + _position_setpoint.setNaN(); // Don't require any position/velocity setpoints + _velocity_setpoint.setNaN(); + _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust +} + +void FlightTaskAutoMapper::_prepareLandSetpoints() +{ + // Keep xy-position and go down with landspeed + float land_speed = _getLandSpeed(); + _position_setpoint = Vector3f(_target(0), _target(1), NAN); + _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed)); + _gear.landing_gear = landing_gear_s::GEAR_DOWN; +} + +void FlightTaskAutoMapper::_prepareTakeoffSetpoints() +{ + // Takeoff is completely defined by target position + _position_setpoint = _target; + _velocity_setpoint = Vector3f(NAN, NAN, NAN); + + _gear.landing_gear = landing_gear_s::GEAR_DOWN; +} + +void FlightTaskAutoMapper::_prepareVelocitySetpoints() +{ + // XY Velocity waypoint + // TODO : Rewiew that. What is the expected behavior? + _position_setpoint = Vector3f(NAN, NAN, _position(2)); + Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed; + _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); +} + +void FlightTaskAutoMapper::_preparePositionSetpoints() +{ + // Simple waypoint navigation: go to xyz target, with standard limitations + _position_setpoint = _target; + _velocity_setpoint.setNaN(); // No special velocity limitations +} + +void FlightTaskAutoMapper::updateParams() +{ + FlightTaskAuto::updateParams(); + + // make sure that alt1 is above alt2 + _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get())); +} + +bool FlightTaskAutoMapper::_highEnoughForLandingGear() +{ + // return true if altitude is above two meters + return _dist_to_ground > 2.0f; +} + +float FlightTaskAutoMapper::_getLandSpeed() +{ + float land_speed = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _constraints.speed_down); + + // user input assisted land speed + if (_param_mpc_land_rc_help.get() + && (_dist_to_ground < _param_mpc_land_alt1.get()) + && _sticks.checkAndSetStickInputs()) { + // stick full up -1 -> stop, stick full down 1 -> double the speed + land_speed *= (1 + _sticks.getPositionExpo()(2)); + } + + return land_speed; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp new file mode 100644 index 0000000..678d45f --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 FlightTaskAutoMapper.hpp + * + * Abstract Flight task which generates local setpoints + * based on the triplet type. + */ + +#pragma once + +#include "FlightTaskAuto.hpp" +#include "Sticks.hpp" + +class FlightTaskAutoMapper : public FlightTaskAuto +{ +public: + FlightTaskAutoMapper(); + virtual ~FlightTaskAutoMapper() = default; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool update() override; + +protected: + + virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */ + + void _prepareIdleSetpoints(); + void _prepareLandSetpoints(); + void _prepareVelocitySetpoints(); + void _prepareTakeoffSetpoints(); + void _preparePositionSetpoints(); + + void updateParams() override; /**< See ModuleParam class */ + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, + (ParamFloat) _param_mpc_land_speed, + (ParamInt) _param_mpc_land_rc_help, + (ParamFloat) + _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed + (ParamFloat) + _param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed + (ParamFloat) _param_mpc_tko_speed, + (ParamFloat) + _param_mpc_tko_ramp_t // time constant for smooth takeoff ramp + ); + +private: + Sticks _sticks; + void _reset(); /**< Resets member variables to current vehicle state */ + WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */ + bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ + float _getLandSpeed(); /**< Returns landing descent speed. */ +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/CMakeLists.txt new file mode 100644 index 0000000..ede1879 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# add the core Flight tasks that the additional flight tasks depend on +add_subdirectory(FlightTask) +add_subdirectory(Utility) +add_subdirectory(Auto) +add_subdirectory(AutoMapper) + +# add all additional flight tasks +foreach(task ${flight_tasks_all}) + add_subdirectory(${task}) +endforeach() diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt new file mode 100644 index 0000000..5ef5777 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskDescend + FlightTaskDescend.cpp +) + +target_link_libraries(FlightTaskDescend PUBLIC FlightTask) +target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp new file mode 100644 index 0000000..106ce9d --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 FlightTaskDescend.cpp + */ + +#include "FlightTaskDescend.hpp" + +bool FlightTaskDescend::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTask::activate(last_setpoint); + // stay level to minimize horizontal drift + _acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN); + // keep heading + _yaw_setpoint = _yaw; + return ret; +} + +bool FlightTaskDescend::update() +{ + bool ret = FlightTask::update(); + + if (PX4_ISFINITE(_velocity(2))) { + // land with landspeed + _velocity_setpoint(2) = _param_mpc_land_speed.get(); + _acceleration_setpoint(2) = NAN; + + } else { + // descend with constant acceleration (crash landing) + _velocity_setpoint(2) = NAN; + _acceleration_setpoint(2) = .3f; + } + + return ret; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp new file mode 100644 index 0000000..5a6edae --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 FlightTaskDescend.hpp + */ + +#pragma once + +#include "FlightTask.hpp" + +class FlightTaskDescend : public FlightTask +{ +public: + FlightTaskDescend() = default; + virtual ~FlightTaskDescend() = default; + + bool update() override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + +private: + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_thr_hover, ///< thrust at hover equilibrium + (ParamFloat) _param_mpc_land_speed ///< velocity for controlled descend + ) +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/CMakeLists.txt new file mode 100644 index 0000000..cdef516 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskFailsafe + FlightTaskFailsafe.cpp +) + +target_link_libraries(FlightTaskFailsafe PUBLIC FlightTask) +target_include_directories(FlightTaskFailsafe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp new file mode 100644 index 0000000..c3ec934 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 FlightTaskFailsafe.cpp + */ + +#include "FlightTaskFailsafe.hpp" + +bool FlightTaskFailsafe::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTask::activate(last_setpoint); + _position_setpoint = _position; + _velocity_setpoint.zero(); + _acceleration_setpoint = matrix::Vector3f(0.f, 0.f, .3f); + _yaw_setpoint = _yaw; + _yawspeed_setpoint = 0.f; + return ret; +} + +bool FlightTaskFailsafe::update() +{ + bool ret = FlightTask::update(); + + if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) { + // stay at current position setpoint + _velocity_setpoint(0) = _velocity_setpoint(1) = 0.f; + _acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.f; + + } else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) { + // don't move along xy + _position_setpoint(0) = _position_setpoint(1) = NAN; + _acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN; + } + + if (PX4_ISFINITE(_position(2))) { + // stay at current altitude setpoint + _velocity_setpoint(2) = 0.f; + _acceleration_setpoint(2) = NAN; + + } else if (PX4_ISFINITE(_velocity(2))) { + // land with landspeed + _velocity_setpoint(2) = _param_mpc_land_speed.get(); + _position_setpoint(2) = NAN; + _acceleration_setpoint(2) = NAN; + } + + return ret; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp new file mode 100644 index 0000000..9c86952 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 FlightTaskFailsafe.hpp + * + */ + +#pragma once + +#include "FlightTask.hpp" + +class FlightTaskFailsafe : public FlightTask +{ +public: + FlightTaskFailsafe() = default; + + virtual ~FlightTaskFailsafe() = default; + bool update() override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + +private: + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_land_speed + ) +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/CMakeLists.txt new file mode 100644 index 0000000..0130303 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# 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(FlightTask + FlightTask.cpp +) + +target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp new file mode 100644 index 0000000..55c78b5 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -0,0 +1,232 @@ +#include "FlightTask.hpp" +#include +#include + +constexpr uint64_t FlightTask::_timeout; +// First index of empty_setpoint corresponds to time-stamp and requires a finite number. +const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}}; + +const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, false, {}}; +const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; + +bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + _resetSetpoints(); + _setDefaultConstraints(); + _time_stamp_activate = hrt_absolute_time(); + _gear = empty_landing_gear_default_keep; + return true; +} + +void FlightTask::reActivate() +{ + activate(empty_setpoint); +} + +bool FlightTask::updateInitialize() +{ + _time_stamp_current = hrt_absolute_time(); + _time = (_time_stamp_current - _time_stamp_activate) / 1e6f; + _deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f; + _time_stamp_last = _time_stamp_current; + + _sub_vehicle_local_position.update(); + _sub_home_position.update(); + + _evaluateVehicleLocalPosition(); + _evaluateVehicleLocalPositionSetpoint(); + _evaluateDistanceToGround(); + return true; +} + +bool FlightTask::update() +{ + _checkEkfResetCounters(); + return true; +} + +void FlightTask::_checkEkfResetCounters() +{ + // Check if a reset event has happened + if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) { + _ekfResetHandlerPositionXY(); + _reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter; + } + + if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) { + _ekfResetHandlerVelocityXY(); + _reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter; + } + + if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) { + _ekfResetHandlerPositionZ(); + _reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter; + } + + if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) { + _ekfResetHandlerVelocityZ(); + _reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter; + } + + if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) { + _ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading); + _reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter; + } +} + +const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint() +{ + /* fill position setpoint message */ + vehicle_local_position_setpoint_s vehicle_local_position_setpoint{}; + vehicle_local_position_setpoint.timestamp = hrt_absolute_time(); + + vehicle_local_position_setpoint.x = _position_setpoint(0); + vehicle_local_position_setpoint.y = _position_setpoint(1); + vehicle_local_position_setpoint.z = _position_setpoint(2); + + vehicle_local_position_setpoint.vx = _velocity_setpoint(0); + vehicle_local_position_setpoint.vy = _velocity_setpoint(1); + vehicle_local_position_setpoint.vz = _velocity_setpoint(2); + + vehicle_local_position_setpoint.yaw = _yaw_setpoint; + vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint; + + _acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration); + _jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk); + + // deprecated, only kept for output logging + matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust); + + return vehicle_local_position_setpoint; +} + +void FlightTask::_resetSetpoints() +{ + _position_setpoint.setNaN(); + _velocity_setpoint.setNaN(); + _acceleration_setpoint.setNaN(); + _jerk_setpoint.setNaN(); + _yaw_setpoint = NAN; + _yawspeed_setpoint = NAN; +} + +void FlightTask::_evaluateVehicleLocalPosition() +{ + _position.setAll(NAN); + _velocity.setAll(NAN); + _yaw = NAN; + _dist_to_bottom = NAN; + + // Only use vehicle-local-position topic fields if the topic is received within a certain timestamp + if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) { + + // yaw + _yaw = _sub_vehicle_local_position.get().heading; + + // position + if (_sub_vehicle_local_position.get().xy_valid) { + _position(0) = _sub_vehicle_local_position.get().x; + _position(1) = _sub_vehicle_local_position.get().y; + } + + if (_sub_vehicle_local_position.get().z_valid) { + _position(2) = _sub_vehicle_local_position.get().z; + } + + // velocity + if (_sub_vehicle_local_position.get().v_xy_valid) { + _velocity(0) = _sub_vehicle_local_position.get().vx; + _velocity(1) = _sub_vehicle_local_position.get().vy; + } + + if (_sub_vehicle_local_position.get().v_z_valid) { + _velocity(2) = _sub_vehicle_local_position.get().vz; + } + + // distance to bottom + if (_sub_vehicle_local_position.get().dist_bottom_valid + && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) { + _dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom; + } + + // global frame reference coordinates to enable conversions + if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) { + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != _sub_vehicle_local_position.get().ref_timestamp)) { + + map_projection_init_timestamped(&_global_local_proj_ref, + _sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon, + _sub_vehicle_local_position.get().ref_timestamp); + + _global_local_alt0 = _sub_vehicle_local_position.get().ref_alt; + } + } + } +} + +void FlightTask::_evaluateVehicleLocalPositionSetpoint() +{ + vehicle_local_position_setpoint_s vehicle_local_position_setpoint; + + // Only use data that is received within a certain timestamp + if (_vehicle_local_position_setpoint_sub.copy(&vehicle_local_position_setpoint) + && (_time_stamp_current - vehicle_local_position_setpoint.timestamp) < _timeout) { + // Inform about the input and output of the velocity controller + // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) + _velocity_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy, + vehicle_local_position_setpoint.vz); + _acceleration_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.acceleration); + + } else { + _velocity_setpoint_feedback.setAll(NAN); + _acceleration_setpoint_feedback.setAll(NAN); + } +} + +void FlightTask::_evaluateDistanceToGround() +{ + // Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available + _dist_to_ground = -_position(2); + + if (PX4_ISFINITE(_dist_to_bottom)) { + _dist_to_ground = _dist_to_bottom; + + } else if (_sub_home_position.get().valid_alt) { + _dist_to_ground = -(_position(2) - _sub_home_position.get().z); + } +} + +void FlightTask::_setDefaultConstraints() +{ + _constraints.speed_xy = _param_mpc_xy_vel_max.get(); + _constraints.speed_up = _param_mpc_z_vel_max_up.get(); + _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + _constraints.want_takeoff = false; +} + +bool FlightTask::_checkTakeoff() +{ + // position setpoint above the minimum altitude + bool position_triggered_takeoff = false; + + if (PX4_ISFINITE(_position_setpoint(2))) { + // minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow + float min_altitude = 0.2f; + const float min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min; + + if (PX4_ISFINITE(min_distance_to_ground)) { + min_altitude = min_distance_to_ground + 0.05f; + } + + position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude); + } + + // upwards velocity setpoint + bool velocity_triggered_takeoff = false; + + if (PX4_ISFINITE(_velocity_setpoint(2))) { + velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f; + } + + return position_triggered_takeoff || velocity_triggered_takeoff; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp new file mode 100644 index 0000000..3752ba0 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -0,0 +1,277 @@ +/**************************************************************************** + * + * Copyright (c) 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 FlightTask.hpp + * + * Abstract base class for different advanced flight tasks like orbit, follow me, ... + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct ekf_reset_counters_s { + uint8_t xy; + uint8_t vxy; + uint8_t z; + uint8_t vz; + uint8_t heading; +}; + +class FlightTask : public ModuleParams +{ +public: + FlightTask() : + ModuleParams(nullptr) + { + _resetSetpoints(); + _constraints = empty_constraints; + } + + virtual ~FlightTask() = default; + + /** + * Call once on the event where you switch to the task + * @param last_setpoint last output of the previous task + * @return true on success, false on error + */ + virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint); + + /** + * Call this to reset an active Flight Task + */ + virtual void reActivate(); + + /** + * To be called to adopt parameters from an arrived vehicle command + * @param command received command message containing the parameters + * @return true if accepted, false if declined + */ + virtual bool applyCommandParameters(const vehicle_command_s &command) { return false; } + + /** + * Call before activate() or update() + * to initialize time and input data + * @return true on success, false on error + */ + virtual bool updateInitialize(); + + /** + * To be called regularly in the control loop cycle to execute the task + * @return true on success, false on error + */ + virtual bool update(); + + /** + * Call after update() + * to constrain the generated setpoints in order to comply + * with the constraints of the current mode + * @return true on success, false on error + */ + virtual bool updateFinalize() { return true; }; + + /** + * Get the output data + * @return task output setpoints that get executed by the positon controller + */ + const vehicle_local_position_setpoint_s getPositionSetpoint(); + + const ekf_reset_counters_s getResetCounters() const { return _reset_counters; } + void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; } + + /** + * Get vehicle constraints. + * The constraints can vary with task. + * @return constraints + */ + const vehicle_constraints_s &getConstraints() { return _constraints; } + + /** + * Get landing gear position. + * The constraints can vary with task. + * @return landing gear + */ + const landing_gear_s &getGear() { return _gear; } + + /** + * Get avoidance desired waypoint + * @return desired waypoints + */ + const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; } + + /** + * Empty setpoint. + * All setpoints are set to NAN. + */ + static const vehicle_local_position_setpoint_s empty_setpoint; + + /** + * Empty constraints. + * All constraints are set to NAN. + */ + static const vehicle_constraints_s empty_constraints; + + /** + * default landing gear state + */ + static const landing_gear_s empty_landing_gear_default_keep; + + /** + * Call this whenever a parameter update notification is received (parameter_update uORB message) + */ + void handleParameterUpdate() + { + updateParams(); + } + + /** + * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy. + * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method. + */ + virtual void setYawHandler(WeatherVane *ext_yaw_handler) {} + + void updateVelocityControllerFeedback(const matrix::Vector3f &vel_sp, + const matrix::Vector3f &acc_sp) + { + _velocity_setpoint_feedback = vel_sp; + _acceleration_setpoint_feedback = acc_sp; + } + +protected: + uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; + uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + + /** Reset all setpoints to NAN */ + void _resetSetpoints(); + + /** Check and update local position */ + void _evaluateVehicleLocalPosition(); + void _evaluateVehicleLocalPositionSetpoint(); + void _evaluateDistanceToGround(); + + /** Set constraints to default values */ + virtual void _setDefaultConstraints(); + + /** Determine when to trigger a takeoff (ignored in flight) */ + virtual bool _checkTakeoff(); + + /** + * Monitor the EKF reset counters and + * call the appropriate handling functions in case of a reset event + * TODO: add the delta values to all the handlers + */ + void _checkEkfResetCounters(); + virtual void _ekfResetHandlerPositionXY() {}; + virtual void _ekfResetHandlerVelocityXY() {}; + virtual void _ekfResetHandlerPositionZ() {}; + virtual void _ekfResetHandlerVelocityZ() {}; + virtual void _ekfResetHandlerHeading(float delta_psi) {}; + + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + + /* Time abstraction */ + static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */ + + float _time{}; /**< passed time in seconds since the task was activated */ + float _deltatime{}; /**< passed time in seconds since the task was last updated */ + + hrt_abstime _time_stamp_activate{}; /**< time stamp when task was activated */ + hrt_abstime _time_stamp_current{}; /**< time stamp at the beginning of the current task update */ + hrt_abstime _time_stamp_last{}; /**< time stamp when task was last updated */ + + /* Current vehicle state */ + matrix::Vector3f _position; /**< current vehicle position */ + matrix::Vector3f _velocity; /**< current vehicle velocity */ + + float _yaw{}; /**< current vehicle yaw heading */ + float _dist_to_bottom{}; /**< current height above ground level */ + float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */ + + /** + * Setpoints which the position controller has to execute. + * Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time. + * If more than one type of setpoint is set, then order of control is a as follow: position, velocity, + * acceleration, thrust. The exception is _position_setpoint together with _velocity_setpoint, where the + * _velocity_setpoint and _acceleration_setpoint are used as feedforward. + * _jerk_setpoint does not executed but just serves as internal state. + */ + matrix::Vector3f _position_setpoint; + matrix::Vector3f _velocity_setpoint; + matrix::Vector3f _velocity_setpoint_feedback; + matrix::Vector3f _acceleration_setpoint; + matrix::Vector3f _acceleration_setpoint_feedback; + matrix::Vector3f _jerk_setpoint; + + float _yaw_setpoint{}; + float _yawspeed_setpoint{}; + + ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets + + /** + * Vehicle constraints. + * The constraints can vary with tasks. + */ + vehicle_constraints_s _constraints{}; + + landing_gear_s _gear{}; + + /** + * Desired waypoints. + * Goals set by the FCU to be sent to the obstacle avoidance system. + */ + vehicle_trajectory_waypoint_s _desired_waypoint{}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, + (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_vel_max_dn, + (ParamFloat) _param_mpc_z_vel_max_up + ) +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt new file mode 100644 index 0000000..5f445d5 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(FlightTaskManualAcceleration + FlightTaskManualAcceleration.cpp +) +target_include_directories(FlightTaskManualAcceleration PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp new file mode 100644 index 0000000..0f9b2e0 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 FlightTaskManualAcceleration.cpp + */ + +#include "FlightTaskManualAcceleration.hpp" + +using namespace matrix; + +FlightTaskManualAcceleration::FlightTaskManualAcceleration() : + _stick_acceleration_xy(this) +{}; + +bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint); + + _stick_acceleration_xy.resetPosition(); + + if (PX4_ISFINITE(last_setpoint.vx) && PX4_ISFINITE(last_setpoint.vy)) { + _stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.vx, last_setpoint.vy)); + + } else { + _stick_acceleration_xy.resetVelocity(_velocity.xy()); + } + + if (PX4_ISFINITE(last_setpoint.acceleration[0]) && PX4_ISFINITE(last_setpoint.acceleration[1])) { + _stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1])); + } + + return ret; +} + +bool FlightTaskManualAcceleration::update() +{ + bool ret = FlightTaskManualAltitudeSmoothVel::update(); + + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, + _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); + _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position, + _velocity_setpoint_feedback.xy(), _deltatime); + _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); + + _constraints.want_takeoff = _checkTakeoff(); + + // check if an external yaw handler is active and if yes, let it update the yaw setpoints + if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) { + _yaw_setpoint = NAN; + + // only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN) + if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { + // vehicle is steady + _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); + } + } + + return ret; +} + +void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY() +{ + _stick_acceleration_xy.resetPosition(); +} + +void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY() +{ + _stick_acceleration_xy.resetVelocity(_velocity.xy()); +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp new file mode 100644 index 0000000..b608dd9 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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 FlightTaskManualPosition.hpp + * + * Flight task for manual position controlled mode. + * + */ + +#pragma once + +#include "FlightTaskManualAltitudeSmoothVel.hpp" +#include "StickAccelerationXY.hpp" +#include "StickYaw.hpp" + +class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel +{ +public: + FlightTaskManualAcceleration(); + virtual ~FlightTaskManualAcceleration() = default; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool update() override; + + /** + * Sets an external yaw handler which can be used to implement a different yaw control strategy. + */ + void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } + +private: + StickAccelerationXY _stick_acceleration_xy; + StickYaw _stick_yaw; + + void _ekfResetHandlerPositionXY() override; + void _ekfResetHandlerVelocityXY() override; + + WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/CMakeLists.txt new file mode 100644 index 0000000..271c697 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskManualAltitude + FlightTaskManualAltitude.cpp +) + +target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTask FlightTaskUtility) +target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp new file mode 100644 index 0000000..c13035f --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -0,0 +1,380 @@ +/**************************************************************************** + * + * 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 FlightManualAltitude.cpp + */ + +#include "FlightTaskManualAltitude.hpp" +#include +#include +#include + +using namespace matrix; + +FlightTaskManualAltitude::FlightTaskManualAltitude() : + _sticks(this) +{} + +bool FlightTaskManualAltitude::updateInitialize() +{ + bool ret = FlightTask::updateInitialize(); + + _sticks.checkAndSetStickInputs(); + + if (_sticks_data_required) { + ret = ret && _sticks.isAvailable(); + } + + // in addition to manual require valid position and velocity in D-direction and valid yaw + return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw); +} + +bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTask::activate(last_setpoint); + _yaw_setpoint = NAN; + _yawspeed_setpoint = 0.f; + _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity + _position_setpoint(2) = _position(2); + _velocity_setpoint(2) = 0.f; + _setDefaultConstraints(); + + _updateConstraintsFromEstimator(); + + _max_speed_up = _constraints.speed_up; + _max_speed_down = _constraints.speed_down; + + return ret; +} + +void FlightTaskManualAltitude::_updateConstraintsFromEstimator() +{ + if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) { + _min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min; + + } else { + _min_distance_to_ground = -INFINITY; + } + + if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) { + _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max; + + } else { + _max_distance_to_ground = INFINITY; + } +} + +void FlightTaskManualAltitude::_scaleSticks() +{ + // Use stick input with deadzone, exponential curve and first order lpf for yawspeed + const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()); + _yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target); + + // Use sticks input with deadzone and exponential curve for vertical velocity + const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; + _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); +} + +float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target) +{ + const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f); + const float alpha = _deltatime / den; + _yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target; + return _yawspeed_filter_state; +} + +void FlightTaskManualAltitude::_updateAltitudeLock() +{ + // Depending on stick inputs and velocity, position is locked. + // If not locked, altitude setpoint is set to NAN. + + // Check if user wants to break + const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON; + + // Check if vehicle has stopped + const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get()); + + // Manage transition between use of distance to ground and distance to local origin + // when terrain hold behaviour has been selected. + if (_param_mpc_alt_mode.get() == 2) { + // Use horizontal speed as a transition criteria + float spd_xy = Vector2f(_velocity).length(); + + // Use presence of horizontal stick inputs as a transition criteria + float stick_xy = Vector2f(_sticks.getPositionExpo().slice<2, 1>(0, 0)).length(); + bool stick_input = stick_xy > 0.001f; + + if (_terrain_hold) { + bool too_fast = spd_xy > _param_mpc_hold_max_xy.get(); + + if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) { + // Stop using distance to ground + _terrain_hold = false; + _terrain_follow = false; + + // Adjust the setpoint to maintain the same height error to reduce control transients + if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) { + _position_setpoint(2) = _position(2) - (_dist_to_ground_lock - _dist_to_bottom); + + } else { + _position_setpoint(2) = _position(2); + } + } + + } else { + bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get(); + + if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) { + // Start using distance to ground + _terrain_hold = true; + _terrain_follow = true; + + // Adjust the setpoint to maintain the same height error to reduce control transients + if (PX4_ISFINITE(_position_setpoint(2))) { + _dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2)); + } + } + } + + } + + if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) { + // terrain following + _terrainFollowing(apply_brake, stopped); + // respect maximum altitude + _respectMaxAltitude(); + + } else { + // normal mode where height is dependent on local frame + + if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) { + // lock position + _position_setpoint(2) = _position(2); + + // Ensure that minimum altitude is respected if + // there is a distance sensor and distance to bottom is below minimum. + if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _min_distance_to_ground) { + _terrainFollowing(apply_brake, stopped); + + } else { + _dist_to_ground_lock = NAN; + } + + } else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) { + // Position is locked but check if a reset event has happened. + // We will shift the setpoints. + if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) { + _position_setpoint(2) = _position(2); + _reset_counter = _sub_vehicle_local_position.get().z_reset_counter; + } + + } else { + // user demands velocity change + _position_setpoint(2) = NAN; + // ensure that maximum altitude is respected + _respectMaxAltitude(); + } + } +} + +void FlightTaskManualAltitude::_respectMinAltitude() +{ + // Height above ground needs to be limited (flow / range-finder) + if (PX4_ISFINITE(_dist_to_bottom) && (_dist_to_bottom < _min_distance_to_ground)) { + // increase altitude to minimum flow distance + _position_setpoint(2) = _position(2) - (_min_distance_to_ground - _dist_to_bottom); + } +} + +void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped) +{ + if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) { + // User wants to break and vehicle reached zero velocity. Lock height to ground. + + // lock position + _position_setpoint(2) = _position(2); + // ensure that minimum altitude is respected + _respectMinAltitude(); + // lock distance to ground but adjust first for minimum altitude + _dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2)); + + } else if (apply_brake && PX4_ISFINITE(_dist_to_ground_lock)) { + // vehicle needs to follow terrain + + // difference between the current distance to ground and the desired distance to ground + const float delta_distance_to_ground = _dist_to_ground_lock - _dist_to_bottom; + // adjust position setpoint for the delta (note: NED frame) + _position_setpoint(2) = _position(2) - delta_distance_to_ground; + + } else { + // user demands velocity change in D-direction + _dist_to_ground_lock = _position_setpoint(2) = NAN; + } +} + +void FlightTaskManualAltitude::_respectMaxAltitude() +{ + if (PX4_ISFINITE(_dist_to_bottom)) { + + // if there is a valid maximum distance to ground, linearly increase speed limit with distance + // below the maximum, preserving control loop vertical position error gain. + if (PX4_ISFINITE(_max_distance_to_ground)) { + _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), + -_max_speed_down, _max_speed_up); + + } else { + _constraints.speed_up = _max_speed_up; + } + + // if distance to bottom exceeded maximum distance, slowly approach maximum distance + if (_dist_to_bottom > _max_distance_to_ground) { + // difference between current distance to ground and maximum distance to ground + const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground; + // set position setpoint to maximum distance to ground + _position_setpoint(2) = _position(2) + delta_distance_to_max; + // limit speed downwards to 0.7m/s + _constraints.speed_down = math::min(_max_speed_down, 0.7f); + + } else { + _constraints.speed_down = _max_speed_down; + } + } +} + +void FlightTaskManualAltitude::_respectGroundSlowdown() +{ + // limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2 + if (PX4_ISFINITE(_dist_to_ground)) { + const float limit_down = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _constraints.speed_down); + const float limit_up = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_tko_speed.get(), _constraints.speed_up); + _velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down); + } +} + +void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v) +{ + float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; + Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); + v(0) = v_r(0); + v(1) = v_r(1); +} + +void FlightTaskManualAltitude::_updateHeadingSetpoints() +{ + if (_isYawInput()) { + _unlockYaw(); + + } else { + _lockYaw(); + } +} + +bool FlightTaskManualAltitude::_isYawInput() +{ + /* + * A threshold larger than FLT_EPSILON is required because the + * _yawspeed_setpoint comes from an IIR filter and takes too much + * time to reach zero. + */ + return fabsf(_yawspeed_setpoint) > 0.001f; +} + +void FlightTaskManualAltitude::_unlockYaw() +{ + // no fixed heading when rotating around yaw by stick + _yaw_setpoint = NAN; +} + +void FlightTaskManualAltitude::_lockYaw() +{ + // hold the current heading when no more rotation commanded + if (!PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint = _yaw; + } +} + +void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) +{ + // Only reset the yaw setpoint when the heading is locked + if (PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint += delta_psi; + } +} + +void FlightTaskManualAltitude::_updateSetpoints() +{ + _updateHeadingSetpoints(); // get yaw setpoint + + // Thrust in xy are extracted directly from stick inputs. A magnitude of + // 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no + // thrust along xy is demanded. The maximum thrust along xy depends on the thrust + // setpoint along z-direction, which is computed in PositionControl.cpp. + + Vector2f sp(_sticks.getPosition().slice<2, 1>(0, 0)); + + _man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get()); + _man_input_filter.update(sp); + sp = _man_input_filter.getState(); + _rotateIntoHeadingFrame(sp); + + if (sp.length() > 1.0f) { + sp.normalize(); + } + + _acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G; + + _updateAltitudeLock(); + _respectGroundSlowdown(); +} + +bool FlightTaskManualAltitude::_checkTakeoff() +{ + // stick is deflected above 65% throttle (throttle stick is in the range [-1,1]) + return _sticks.getPosition()(2) < -0.3f; +} + +bool FlightTaskManualAltitude::update() +{ + bool ret = FlightTask::update(); + _updateConstraintsFromEstimator(); + _scaleSticks(); + _updateSetpoints(); + _constraints.want_takeoff = _checkTakeoff(); + + return ret; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp new file mode 100644 index 0000000..c4b1420 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * 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 FlightManualAltitude.hpp + * + * Flight task for manual controlled altitude. + */ + +#pragma once + +#include "FlightTask.hpp" +#include "Sticks.hpp" +#include +#include + +class FlightTaskManualAltitude : public FlightTask +{ +public: + FlightTaskManualAltitude(); + virtual ~FlightTaskManualAltitude() = default; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool updateInitialize() override; + bool update() override; + +protected: + void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ + void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ + virtual void _updateSetpoints(); /**< updates all setpoints */ + virtual void _scaleSticks(); /**< scales sticks to velocity in z */ + bool _checkTakeoff() override; + void _updateConstraintsFromEstimator(); + + /** + * rotates vector into local frame + */ + void _rotateIntoHeadingFrame(matrix::Vector2f &vec); + + /** + * Check and sets for position lock. + * If sticks are at center position, the vehicle + * will exit velocity control and enter position control. + */ + void _updateAltitudeLock(); + + Sticks _sticks; + bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_hold_max_z, + (ParamInt) _param_mpc_alt_mode, + (ParamFloat) _param_mpc_hold_max_xy, + (ParamFloat) _param_mpc_z_p, /**< position controller altitude propotional gain */ + (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ + (ParamFloat) _param_mpc_man_y_tau, + (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ + (ParamFloat) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */ + (ParamFloat) _param_mpc_land_alt2, /**< altitude below wich to land with land speed */ + (ParamFloat) + _param_mpc_land_speed, /**< desired downwards speed when approaching the ground */ + (ParamFloat) + _param_mpc_tko_speed, /**< desired upwards speed when still close to the ground */ + (ParamFloat) _param_mc_man_tilt_tau + ) +private: + bool _isYawInput(); + void _unlockYaw(); + void _lockYaw(); + + /** + * Filter between stick input and yaw setpoint + * + * @param yawspeed_target yaw setpoint desired by the stick + * @return filtered value from independent filter state + */ + float _applyYawspeedFilter(float yawspeed_target); + + /** + * Terrain following. + * During terrain following, the position setpoint is adjusted + * such that the distance to ground is kept constant. + * @param apply_brake is true if user wants to break + * @param stopped is true if vehicle has stopped moving in D-direction + */ + void _terrainFollowing(bool apply_brake, bool stopped); + + /** + * Minimum Altitude during range sensor operation. + * If a range sensor is used for altitude estimates, for + * best operation a minimum altitude is required. The minimum + * altitude is only enforced during altitude lock. + */ + void _respectMinAltitude(); + + void _respectMaxAltitude(); + + /** + * Sets downwards velocity constraint based on the distance to ground. + * To ensure a slowdown to land speed before hitting the ground. + */ + void _respectGroundSlowdown(); + + void setGearAccordingToSwitch(); + + float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */ + uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ + float _max_speed_up = 10.0f; + float _max_speed_down = 1.0f; + bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ + bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ + + float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */ + float _max_distance_to_ground{(float)INFINITY}; /**< max distance to ground constraint */ + + /** + * Distance to ground during terrain following. + * If user does not demand velocity change in D-direction and the vehcile + * is in terrain-following mode, then height to ground will be locked to + * _dist_to_ground_lock. + */ + float _dist_to_ground_lock = NAN; + + AlphaFilter _man_input_filter; +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/CMakeLists.txt new file mode 100644 index 0000000..30d4038 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskManualAltitudeSmoothVel + FlightTaskManualAltitudeSmoothVel.cpp +) + +target_link_libraries(FlightTaskManualAltitudeSmoothVel PUBLIC FlightTaskManualAltitude FlightTaskUtility) +target_include_directories(FlightTaskManualAltitudeSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp new file mode 100644 index 0000000..da460ad --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 FlightManualAltitude.cpp + */ + +#include "FlightTaskManualAltitudeSmoothVel.hpp" + +#include + +using namespace matrix; + +bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTaskManualAltitude::activate(last_setpoint); + + // Check if the previous FlightTask provided setpoints + + // If the position setpoint is unknown, set to the current postion + float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2); + + // If the velocity setpoint is unknown, set to the current velocity + float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2); + + // No acceleration estimate available, set to zero if the setpoint is NAN + float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f; + + _smoothing.reset(az_sp_last, vz_sp_last, z_sp_last); + + return ret; +} + +void FlightTaskManualAltitudeSmoothVel::reActivate() +{ + FlightTaskManualAltitude::reActivate(); + // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly + // using the generated jerk, reset the z derivatives to zero + _smoothing.reset(0.f, 0.f, _position(2)); +} + +void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ() +{ + _smoothing.setCurrentPosition(_position(2)); +} + +void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerVelocityZ() +{ + _smoothing.setCurrentVelocity(_velocity(2)); +} + +void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() +{ + // Set max accel/vel/jerk + // Has to be done before _updateTrajectories() + _updateTrajConstraints(); + + _smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2)); + _smoothing.setCurrentPositionEstimate(_position(2)); + + // Get yaw setpoint, un-smoothed position setpoints + FlightTaskManualAltitude::_updateSetpoints(); + + _smoothing.update(_deltatime, _velocity_setpoint(2)); + + // Fill the jerk, acceleration, velocity and position setpoint vectors + _setOutputState(); +} + +void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints() +{ + _smoothing.setMaxJerk(_param_mpc_jerk_max.get()); + + _smoothing.setMaxAccelUp(_param_mpc_acc_up_max.get()); + _smoothing.setMaxVelUp(_constraints.speed_up); + + _smoothing.setMaxAccelDown(_param_mpc_acc_down_max.get()); + _smoothing.setMaxVelDown(_constraints.speed_down); +} + + +void FlightTaskManualAltitudeSmoothVel::_setOutputState() +{ + _jerk_setpoint(2) = _smoothing.getCurrentJerk(); + _acceleration_setpoint(2) = _smoothing.getCurrentAcceleration(); + _velocity_setpoint(2) = _smoothing.getCurrentVelocity(); + _position_setpoint(2) = _smoothing.getCurrentPosition(); +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp new file mode 100644 index 0000000..7a681ac --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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 FlightManualAltitudeSmoothVel.hpp + * + * Flight task for manual controlled altitude using the velocity smoothing library + */ + +#pragma once + +#include "FlightTaskManualAltitude.hpp" +#include "ManualVelocitySmoothingZ.hpp" + +class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude +{ +public: + FlightTaskManualAltitudeSmoothVel() = default; + virtual ~FlightTaskManualAltitudeSmoothVel() = default; + + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + void reActivate() override; + +protected: + virtual void _updateSetpoints() override; + + /** Reset position or velocity setpoints in case of EKF reset event */ + void _ekfResetHandlerPositionZ() override; + void _ekfResetHandlerVelocityZ() override; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_down_max + ) + +private: + void _updateTrajConstraints(); + void _setOutputState(); + + ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt new file mode 100644 index 0000000..6125d54 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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(FlightTaskManualPosition + FlightTaskManualPosition.cpp + FlightTaskManualPosition.hpp +) + +target_link_libraries(FlightTaskManualPosition + PRIVATE + CollisionPrevention + PUBLIC + FlightTaskManualAltitude +) + +target_include_directories(FlightTaskManualPosition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp new file mode 100644 index 0000000..5ef59be --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 FlightTaskManualPosition.cpp + */ + +#include "FlightTaskManualPosition.hpp" +#include +#include + +using namespace matrix; + +FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this) +{ + +} + +bool FlightTaskManualPosition::updateInitialize() +{ + bool ret = FlightTaskManualAltitude::updateInitialize(); + // require valid position / velocity in xy + return ret && PX4_ISFINITE(_position(0)) + && PX4_ISFINITE(_position(1)) + && PX4_ISFINITE(_velocity(0)) + && PX4_ISFINITE(_velocity(1)); +} + +bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + // all requirements from altitude-mode still have to hold + bool ret = FlightTaskManualAltitude::activate(last_setpoint); + + // set task specific constraint + if (_constraints.speed_xy >= _param_mpc_vel_manual.get()) { + _constraints.speed_xy = _param_mpc_vel_manual.get(); + } + + _position_setpoint(0) = _position(0); + _position_setpoint(1) = _position(1); + _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; + _velocity_scale = _constraints.speed_xy; + + // for position-controlled mode, we need a valid position and velocity state + // in NE-direction + return ret; +} + +void FlightTaskManualPosition::_scaleSticks() +{ + /* Use same scaling as for FlightTaskManualAltitude */ + FlightTaskManualAltitude::_scaleSticks(); + + /* Constrain length of stick inputs to 1 for xy*/ + Vector2f stick_xy = _sticks.getPositionExpo().slice<2, 1>(0, 0); + + const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f); + + if (mag > FLT_EPSILON) { + stick_xy = stick_xy.normalized() * mag; + } + + const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max; + + if (PX4_ISFINITE(max_speed_from_estimator)) { + // use the minimum of the estimator and user specified limit + _velocity_scale = fminf(_constraints.speed_xy, max_speed_from_estimator); + // Allow for a minimum of 0.3 m/s for repositioning + _velocity_scale = fmaxf(_velocity_scale, 0.3f); + + } else { + _velocity_scale = _constraints.speed_xy; + } + + // scale velocity to its maximum limits + Vector2f vel_sp_xy = stick_xy * _velocity_scale; + + /* Rotate setpoint into local frame. */ + _rotateIntoHeadingFrame(vel_sp_xy); + + // collision prevention + if (_collision_prevention.is_active()) { + _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, _position.xy(), _velocity.xy()); + } + + _velocity_setpoint.xy() = vel_sp_xy; +} + +void FlightTaskManualPosition::_updateXYlock() +{ + /* If position lock is not active, position setpoint is set to NAN.*/ + const float vel_xy_norm = Vector2f(_velocity).length(); + const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON; + const bool stopped = (_param_mpc_hold_max_xy.get() < FLT_EPSILON || vel_xy_norm < _param_mpc_hold_max_xy.get()); + + if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) { + _position_setpoint(0) = _position(0); + _position_setpoint(1) = _position(1); + + } else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) { + // Position is locked but check if a reset event has happened. + // We will shift the setpoints. + if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counter) { + _position_setpoint(0) = _position(0); + _position_setpoint(1) = _position(1); + _reset_counter = _sub_vehicle_local_position.get().xy_reset_counter; + } + + } else { + /* don't lock*/ + _position_setpoint(0) = NAN; + _position_setpoint(1) = NAN; + } +} + +void FlightTaskManualPosition::_updateSetpoints() +{ + FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction + _acceleration_setpoint.setNaN(); // don't use the horizontal setpoints from FlightTaskAltitude + + _updateXYlock(); // check for position lock + + // check if an external yaw handler is active and if yes, let it update the yaw setpoints + if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) { + _yaw_setpoint = NAN; + + // only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN) + if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { + // vehicle is steady + _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); + } + } +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp new file mode 100644 index 0000000..b0db7c3 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 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 FlightTaskManualPosition.hpp + * + * Flight task for manual position controlled mode. + * + */ + +#pragma once + +#include +#include "FlightTaskManualAltitude.hpp" + +class FlightTaskManualPosition : public FlightTaskManualAltitude +{ +public: + FlightTaskManualPosition(); + + virtual ~FlightTaskManualPosition() = default; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool updateInitialize() override; + + /** + * Sets an external yaw handler which can be used to implement a different yaw control strategy. + */ + void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } + + +protected: + void _updateXYlock(); /**< applies position lock based on stick and velocity */ + void _updateSetpoints() override; + void _scaleSticks() override; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_acc_hor_max, + (ParamFloat) _param_mpc_hold_max_xy + ) +private: + float _velocity_scale{0.0f}; //scales the stick input to velocity + uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + + WeatherVane *_weathervane_yaw_handler = + nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + + CollisionPrevention _collision_prevention; /**< collision avoidance setpoint amendment */ +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt new file mode 100644 index 0000000..8d4489f --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskManualPositionSmoothVel + FlightTaskManualPositionSmoothVel.cpp +) + +target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility) +target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp new file mode 100644 index 0000000..964cf0a --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 "FlightTaskManualPositionSmoothVel.hpp" + +#include +#include + +using namespace matrix; + +bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTaskManualPosition::activate(last_setpoint); + + // Check if the previous FlightTask provided setpoints + Vector3f accel_prev{last_setpoint.acceleration}; + Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz}; + Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z}; + + for (int i = 0; i < 3; i++) { + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } + + // No acceleration estimate available, set to zero if the setpoint is NAN + if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } + } + + _smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev}); + _smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2)); + + return ret; +} + +void FlightTaskManualPositionSmoothVel::reActivate() +{ + FlightTaskManualPosition::reActivate(); + // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly + // using the generated jerk, reset the z derivatives to zero + _smoothing_xy.reset(Vector2f(), _velocity.xy(), _position.xy()); + _smoothing_z.reset(0.f, 0.f, _position(2)); +} + +void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY() +{ + _smoothing_xy.setCurrentPosition(_position.xy()); +} + +void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY() +{ + _smoothing_xy.setCurrentVelocity(_velocity.xy()); +} + +void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ() +{ + _smoothing_z.setCurrentPosition(_position(2)); +} + +void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ() +{ + _smoothing_z.setCurrentVelocity(_velocity(2)); +} + +void FlightTaskManualPositionSmoothVel::_updateSetpoints() +{ + // Set max accel/vel/jerk + // Has to be done before _updateTrajectories() + _updateTrajConstraints(); + _updateTrajVelFeedback(); + _updateTrajCurrentPositionEstimate(); + + // Get yaw setpoint, un-smoothed position setpoints + FlightTaskManualPosition::_updateSetpoints(); + + _updateTrajectories(_velocity_setpoint); + + // Fill the jerk, acceleration, velocity and position setpoint vectors + _setOutputState(); +} + +void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() +{ + _updateTrajConstraintsXY(); + _updateTrajConstraintsZ(); +} + +void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY() +{ + _smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get()); + _smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get()); + _smoothing_xy.setMaxVel(_constraints.speed_xy); +} + +void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ() +{ + _smoothing_z.setMaxJerk(_param_mpc_jerk_max.get()); + + _smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get()); + _smoothing_z.setMaxVelUp(_constraints.speed_up); + + _smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get()); + _smoothing_z.setMaxVelDown(_constraints.speed_down); +} + +void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback() +{ + _smoothing_xy.setVelSpFeedback(_velocity_setpoint_feedback.xy()); + _smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2)); +} + +void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate() +{ + _smoothing_xy.setCurrentPositionEstimate(_position.xy()); + _smoothing_z.setCurrentPositionEstimate(_position(2)); +} + +void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target) +{ + _smoothing_xy.update(_deltatime, vel_target.xy()); + _smoothing_z.update(_deltatime, vel_target(2)); +} + +void FlightTaskManualPositionSmoothVel::_setOutputState() +{ + _setOutputStateXY(); + _setOutputStateZ(); +} + +void FlightTaskManualPositionSmoothVel::_setOutputStateXY() +{ + _jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk(); + _acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration(); + _velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity(); + _position_setpoint.xy() = _smoothing_xy.getCurrentPosition(); +} + +void FlightTaskManualPositionSmoothVel::_setOutputStateZ() +{ + _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); + _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); + _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); + _position_setpoint(2) = _smoothing_z.getCurrentPosition(); +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp new file mode 100644 index 0000000..2338a41 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 FlightTaskManualPositionSmoothVel.hpp + * + * Flight task for smooth manual controlled position. + */ + +#pragma once + +#include "FlightTaskManualPosition.hpp" +#include "ManualVelocitySmoothingXY.hpp" +#include "ManualVelocitySmoothingZ.hpp" + +using matrix::Vector2f; +using matrix::Vector3f; + +class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition +{ +public: + FlightTaskManualPositionSmoothVel() = default; + + virtual ~FlightTaskManualPositionSmoothVel() = default; + + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + void reActivate() override; + +protected: + + virtual void _updateSetpoints() override; + + /** Reset position or velocity setpoints in case of EKF reset event */ + void _ekfResetHandlerPositionXY() override; + void _ekfResetHandlerVelocityXY() override; + void _ekfResetHandlerPositionZ() override; + void _ekfResetHandlerVelocityZ() override; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_down_max + ) + +private: + void _updateTrajConstraints(); + void _updateTrajConstraintsXY(); + void _updateTrajConstraintsZ(); + + void _updateTrajVelFeedback(); + void _updateTrajCurrentPositionEstimate(); + + void _updateTrajectories(Vector3f vel_target); + + void _setOutputState(); + void _setOutputStateXY(); + void _setOutputStateZ(); + + ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions + ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/CMakeLists.txt new file mode 100644 index 0000000..d925f87 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskOrbit + FlightTaskOrbit.cpp +) + +target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel SlewRate) +target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp new file mode 100644 index 0000000..1993231 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -0,0 +1,281 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 FlightTaskOrbit.cpp + */ + +#include "FlightTaskOrbit.hpp" + +#include +#include + +using namespace matrix; + +FlightTaskOrbit::FlightTaskOrbit() : _circle_approach_line(_position) +{ + _sticks_data_required = false; +} + +bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) +{ + bool ret = true; + // save previous velocity and roatation direction + float v = fabsf(_v); + bool clockwise = _v > 0; + + // commanded radius + if (PX4_ISFINITE(command.param1)) { + clockwise = command.param1 > 0; + const float r = fabsf(command.param1); + ret = ret && setRadius(r); + } + + // commanded velocity, take sign of radius as rotation direction + if (PX4_ISFINITE(command.param2)) { + v = command.param2; + } + + ret = ret && setVelocity(v * (clockwise ? 1.f : -1.f)); + + // commanded heading behaviour + if (PX4_ISFINITE(command.param3)) { + _yaw_behaviour = command.param3; + } + + // save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING + _initial_heading = _yaw; + + // commanded center coordinates + if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) { + if (map_projection_initialized(&_global_local_proj_ref)) { + map_projection_project(&_global_local_proj_ref, + command.param5, command.param6, + &_center(0), &_center(1)); + + } else { + ret = false; + } + } + + // commanded altitude + if (PX4_ISFINITE(command.param7)) { + if (map_projection_initialized(&_global_local_proj_ref)) { + _position_setpoint(2) = _global_local_alt0 - command.param7; + + } else { + ret = false; + } + } + + // perpendicularly approach the orbit circle again when new parameters get commanded + _in_circle_approach = true; + _circle_approach_line.reset(); + + return ret; +} + +bool FlightTaskOrbit::sendTelemetry() +{ + orbit_status_s orbit_status{}; + orbit_status.radius = math::signNoZero(_v) * _r; + orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL + orbit_status.yaw_behaviour = _yaw_behaviour; + + if (map_projection_initialized(&_global_local_proj_ref)) { + // local -> global + map_projection_reproject(&_global_local_proj_ref, _center(0), _center(1), &orbit_status.x, &orbit_status.y); + orbit_status.z = _global_local_alt0 - _position_setpoint(2); + + } else { + return false; // don't send the message if the transformation failed + } + + orbit_status.timestamp = hrt_absolute_time(); + _orbit_status_pub.publish(orbit_status); + + return true; +} + +bool FlightTaskOrbit::setRadius(float r) +{ + // clip the radius to be within range + r = math::constrain(r, _radius_min, _radius_max); + + // small radius is more important than high velocity for safety + if (!checkAcceleration(r, _v, _acceleration_max)) { + _v = sign(_v) * sqrtf(_acceleration_max * r); + } + + if (fabs(_r - r) > FLT_EPSILON) { + _circle_approach_line.reset(); + } + + _r = r; + return true; +} + +bool FlightTaskOrbit::setVelocity(const float v) +{ + if (fabs(v) < _velocity_max && + checkAcceleration(_r, v, _acceleration_max)) { + _v = v; + return true; + } + + return false; +} + +bool FlightTaskOrbit::checkAcceleration(float r, float v, float a) +{ + return v * v < a * r; +} + +bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint); + _r = _radius_min; + _v = 1.f; + _center = _position.xy(); + _initial_heading = _yaw; + _slew_rate_yaw.setForcedValue(_yaw); + _slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get())); + + // need a valid position and velocity + ret = ret && PX4_ISFINITE(_position(0)) + && PX4_ISFINITE(_position(1)) + && PX4_ISFINITE(_position(2)) + && PX4_ISFINITE(_velocity(0)) + && PX4_ISFINITE(_velocity(1)) + && PX4_ISFINITE(_velocity(2)); + + return ret; +} + +bool FlightTaskOrbit::update() +{ + // update altitude + bool ret = FlightTaskManualAltitudeSmoothVel::update(); + + // stick input adjusts parameters within a fixed time frame + const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f); + const float v = _v - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f); + + setRadius(r); + setVelocity(v); + + const Vector2f center_to_position = Vector2f(_position) - _center; + + if (_in_circle_approach) { + generate_circle_approach_setpoints(center_to_position); + + } else { + generate_circle_setpoints(center_to_position); + generate_circle_yaw_setpoints(center_to_position); + } + + // Apply yaw smoothing + _yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime); + + // publish information to UI + sendTelemetry(); + + return ret; +} + +void FlightTaskOrbit::generate_circle_approach_setpoints(const Vector2f ¢er_to_position) +{ + const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero(); + + if (_circle_approach_line.isEndReached()) { + // calculate target point on circle and plan a line trajectory + const Vector2f closest_circle_point = Vector2f(_position) + start_to_circle; + const Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2)); + _circle_approach_line.setLineFromTo(_position, target); + _circle_approach_line.setSpeed(_param_mpc_xy_cruise.get()); + } + + _yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0)); + + // follow the planned line and switch to orbiting once the circle is reached + _circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint); + _in_circle_approach = !_circle_approach_line.isEndReached(); +} + +void FlightTaskOrbit::generate_circle_setpoints(const Vector2f ¢er_to_position) +{ + // xy velocity to go around in a circle + Vector2f velocity_xy(-center_to_position(1), center_to_position(0)); + velocity_xy = velocity_xy.unit_or_zero(); + velocity_xy *= _v; + + // xy velocity adjustment to stay on the radius distance + velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero(); + + _position_setpoint(0) = _position_setpoint(1) = NAN; + _velocity_setpoint.xy() = velocity_xy; + _acceleration_setpoint.xy() = -center_to_position.unit_or_zero() * _v * _v / _r; +} + +void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f ¢er_to_position) +{ + switch (_yaw_behaviour) { + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING: + // make vehicle keep the same heading as when the orbit was commanded + _yaw_setpoint = _initial_heading; + _yawspeed_setpoint = NAN; + break; + + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED: + // no yaw setpoint + _yaw_setpoint = NAN; + _yawspeed_setpoint = NAN; + break; + + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE: + _yaw_setpoint = atan2f(sign(_v) * center_to_position(0), -sign(_v) * center_to_position(1)); + _yawspeed_setpoint = _v / _r; + break; + + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED: + // inherit setpoint from altitude flight task + break; + + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER: + default: + _yaw_setpoint = atan2f(-center_to_position(1), -center_to_position(0)); + // yawspeed feed-forward because we know the necessary angular rate + _yawspeed_setpoint = _v / _r; + break; + } +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp new file mode 100644 index 0000000..ef26880 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file FlightTaskOrbit.hpp + * + * Flight task for orbiting in a circle around a target position + * + * @author Matthias Grob + */ + +#pragma once + +#include "FlightTaskManualAltitudeSmoothVel.hpp" +#include +#include +#include "StraightLine.hpp" +#include + +class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel +{ +public: + FlightTaskOrbit(); + virtual ~FlightTaskOrbit() = default; + + bool applyCommandParameters(const vehicle_command_s &command) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool update() override; + + /** + * Check the feasibility of orbit parameters with respect to + * centripetal acceleration a = v^2 / r + * @param r desired radius + * @param v desired velocity + * @param a maximal allowed acceleration + * @return true on success, false if value not accepted + */ + bool checkAcceleration(float r, float v, float a); + +protected: + /** + * Send out telemetry information for the log and MAVLink. + * @return true on success, false on error + */ + bool sendTelemetry(); + + /** + * Change the radius of the circle. + * @param r desired new radius + * @return true on success, false if value not accepted + */ + bool setRadius(const float r); + + /** + * Change the velocity of the vehicle on the circle. + * @param v desired new velocity + * @return true on success, false if value not accepted + */ + bool setVelocity(const float v); + +private: + /** generates setpoints to smoothly reach the closest point on the circle when starting from far away */ + void generate_circle_approach_setpoints(const matrix::Vector2f ¢er_to_position); + /** generates xy setpoints to make the vehicle orbit */ + void generate_circle_setpoints(const matrix::Vector2f ¢er_to_position); + /** generates yaw setpoints to control the vehicle's heading */ + void generate_circle_yaw_setpoints(const matrix::Vector2f ¢er_to_position); + + float _r = 0.f; /**< radius with which to orbit the target */ + float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */ + matrix::Vector2f _center; /**< local frame coordinates of the center point */ + + bool _in_circle_approach = false; + StraightLine _circle_approach_line; + + // TODO: create/use parameters for limits + const float _radius_min = 1.f; + const float _radius_max = 100.f; + const float _velocity_max = 10.f; + const float _acceleration_max = 2.f; + + /** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */ + int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER; + float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ + SlewRateYaw _slew_rate_yaw; + + uORB::Publication _orbit_status_pub{ORB_ID(orbit_status)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_xy_cruise, /**< cruise speed for circle approach */ + (ParamFloat) _param_mpc_yawrauto_max + ) +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt new file mode 100644 index 0000000..372ebf5 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(FlightTaskTransition + FlightTaskTransition.cpp +) + +target_link_libraries(FlightTaskTransition PUBLIC FlightTask) +target_include_directories(FlightTaskTransition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp new file mode 100644 index 0000000..e2737a8 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 FlightTaskTranstion.cpp + */ + +#include "FlightTaskTransition.hpp" + +bool FlightTaskTransition::updateInitialize() +{ + return FlightTask::updateInitialize(); +} + +bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + return FlightTask::activate(last_setpoint); +} + +bool FlightTaskTransition::update() +{ + bool ret = FlightTask::update(); + _acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f); + // demand zero vertical velocity and level attitude + // tailsitters will override attitude and thrust setpoint + // tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint + _position_setpoint.setAll(NAN); + _velocity_setpoint(2) = 0.0f; + + _yaw_setpoint = NAN; + return ret; +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp new file mode 100644 index 0000000..ec71772 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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 FlightTaskTransition.hpp + * + * Flight task for automatic VTOL transitions between hover and forward flight and vice versa. + */ + +#pragma once + +#include "FlightTask.hpp" + +class FlightTaskTransition : public FlightTask +{ +public: + FlightTaskTransition() = default; + + virtual ~FlightTaskTransition() = default; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool updateInitialize() override; + bool update() override; +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt new file mode 100644 index 0000000..a1d7c5b --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2018 - 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_library(FlightTaskUtility + StraightLine.cpp + ManualVelocitySmoothingXY.cpp + ManualVelocitySmoothingZ.cpp + Sticks.cpp + StickAccelerationXY.cpp + StickYaw.cpp +) + +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning) +target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC ManualVelocitySmoothingXYTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Makefile b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Makefile new file mode 100644 index 0000000..df018d7 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Makefile @@ -0,0 +1,12 @@ + +.PHONY: all tests clean +all: test_velocity_smoothing + +test_velocity_smoothing: test_velocity_smoothing.cpp VelocitySmoothing.cpp + @g++ $^ -std=c++11 -I ../../../ -o $@ + +tests: test_velocity_smoothing + @echo "Test velocity smoothing" + +clean: + @rm test_velocity_smoothing diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.cpp new file mode 100644 index 0000000..0cdc63b --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 "ManualVelocitySmoothingXY.hpp" + +#include +#include + +using namespace matrix; + +void ManualVelocitySmoothingXY::reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos) +{ + for (int i = 0; i < 2; i++) { + _trajectory[i].reset(accel(i), vel(i), pos(i)); + } + + resetPositionLock(); +} + +void ManualVelocitySmoothingXY::resetPositionLock() +{ + _position_lock_active = false; + _position_setpoint_locked(0) = NAN; + _position_setpoint_locked(1) = NAN; +} + +void ManualVelocitySmoothingXY::update(float dt, const Vector2f &velocity_target) +{ + // Update state + updateTrajectories(dt); + + // Lock or unlock position + // Has to be done before _updateTrajDurations() + checkPositionLock(velocity_target); + + // Update durations and sync XY + updateTrajDurations(velocity_target); +} + +void ManualVelocitySmoothingXY::updateTrajectories(float dt) +{ + for (int i = 0; i < 2; ++i) { + _trajectory[i].updateTraj(dt); + + _state.j(i) = _trajectory[i].getCurrentJerk(); + _state.a(i) = _trajectory[i].getCurrentAcceleration(); + _state.v(i) = _trajectory[i].getCurrentVelocity(); + _state.x(i) = _trajectory[i].getCurrentPosition(); + } +} + +void ManualVelocitySmoothingXY::updateTrajDurations(const Vector2f &velocity_target) +{ + for (int i = 0; i < 2; ++i) { + _trajectory[i].updateDurations(velocity_target(i)); + } + + VelocitySmoothing::timeSynchronization(_trajectory, 2); +} + +void ManualVelocitySmoothingXY::checkPositionLock(const Vector2f &velocity_target) +{ + /** + * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint + * is continuous. We know that the output of the position loop (part of the velocity setpoint) + * will suddenly become null + * and only the feedforward (generated by this flight task) will remain. + * This is why the previous input of the velocity controller + * is used to set current velocity of the trajectory. + */ + if (_state.v.length() < 0.1f && + _state.a.length() < .2f && + velocity_target.length() <= FLT_EPSILON) { + // Lock position + _position_lock_active = true; + _position_setpoint_locked = _state.x; + + } else { + // Unlock position + if (_position_lock_active) { + // Start the trajectory at the current velocity setpoint + _trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); + _trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1)); + _state.v = _velocity_setpoint_feedback; + resetPositionLock(); + } + + _trajectory[0].setCurrentPosition(_position_estimate(0)); + _trajectory[1].setCurrentPosition(_position_estimate(1)); + } +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.hpp new file mode 100644 index 0000000..f9d3b89 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 ManualVelocitySmoothingXY.hpp + * + */ + +#pragma once + +#include + +#include + +using matrix::Vector2f; + +class ManualVelocitySmoothingXY final +{ +public: + ManualVelocitySmoothingXY() = default; + ~ManualVelocitySmoothingXY() = default; + + void reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos); + void update(float dt, const Vector2f &velocity_target); + + void setVelSpFeedback(const Vector2f &fb) { _velocity_setpoint_feedback = fb; } + + void setMaxJerk(const float max_jerk) + { + _trajectory[0].setMaxJerk(max_jerk); + _trajectory[1].setMaxJerk(max_jerk); + } + float getMaxJerk() const { return _trajectory[0].getMaxJerk(); } + + void setMaxAccel(const float max_accel) + { + _trajectory[0].setMaxAccel(max_accel); + _trajectory[1].setMaxAccel(max_accel); + } + float getMaxAccel() const { return _trajectory[0].getMaxAccel(); } + + void setMaxVel(const float max_vel) + { + _trajectory[0].setMaxVel(max_vel); + _trajectory[1].setMaxVel(max_vel); + } + float getMaxVel() const { return _trajectory[0].getMaxVel(); } + + Vector2f getCurrentJerk() const { return _state.j; } + Vector2f getCurrentAcceleration() const { return _state.a; } + + void setCurrentVelocity(const Vector2f &vel) + { + _state.v = vel; + _trajectory[0].setCurrentVelocity(vel(0)); + _trajectory[1].setCurrentVelocity(vel(1)); + } + Vector2f getCurrentVelocity() const { return _state.v; } + + void setCurrentPosition(const Vector2f &pos) + { + _state.x = pos; + _trajectory[0].setCurrentPosition(pos(0)); + _trajectory[1].setCurrentPosition(pos(1)); + _position_estimate = pos; + + if (_position_lock_active) { + _position_setpoint_locked = pos; + } + } + Vector2f getCurrentPosition() const { return _position_setpoint_locked; } + + void setCurrentPositionEstimate(const Vector2f &pos) { _position_estimate = pos; } + +private: + void resetPositionLock(); + void updateTrajectories(float dt); + void checkPositionLock(const Vector2f &velocity_target); + void updateTrajDurations(const Vector2f &velocity_target); + + VelocitySmoothing _trajectory[2]; ///< Trajectory in x and y directions + + bool _position_lock_active{false}; + + Vector2f _position_setpoint_locked{}; + + Vector2f _velocity_setpoint_feedback{}; + Vector2f _position_estimate{}; + + struct { + Vector2f j; + Vector2f a; + Vector2f v; + Vector2f x; + } _state{}; +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXYTest.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXYTest.cpp new file mode 100644 index 0000000..e662183 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXYTest.cpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Test code for the Manual Velocity Smoothing library + * Run this test only using make tests TESTFILTER=ManualVelocitySmoothing + */ + +#include +#include + +#include "ManualVelocitySmoothingXY.hpp" + +using namespace matrix; + +class ManualVelocitySmoothingXYTest : public ::testing::Test +{ +public: + ManualVelocitySmoothingXY _smoothing; +}; + + +TEST_F(ManualVelocitySmoothingXYTest, setGet) +{ + // GIVEN: Some max values + _smoothing.setMaxJerk(11.f); + _smoothing.setMaxAccel(7.f); + _smoothing.setMaxVel(5.f); + + // THEN: We should be able to get them back + EXPECT_FLOAT_EQ(_smoothing.getMaxJerk(), 11.f); + EXPECT_FLOAT_EQ(_smoothing.getMaxAccel(), 7.f); + EXPECT_FLOAT_EQ(_smoothing.getMaxVel(), 5.f); +} + +TEST_F(ManualVelocitySmoothingXYTest, getCurrentState) +{ + // GIVEN: the initial conditions + Vector2f v0(11.f, 13.f); + _smoothing.setCurrentVelocity(v0); + + // WHEN: we get the current state + Vector3f j_end; + Vector3f a_end; + Vector3f v_end; + Vector3f x_end; + j_end.xy() = _smoothing.getCurrentJerk(); + a_end.xy() = _smoothing.getCurrentAcceleration(); + v_end.xy() = _smoothing.getCurrentVelocity(); + x_end.xy() = _smoothing.getCurrentPosition(); + + // THEN: the returned values should match the input + EXPECT_EQ(j_end, Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(a_end, Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(v_end, Vector3f(11.f, 13.f, 0.f)); + EXPECT_EQ(x_end, Vector3f(0.f, 0.f, 0.f)); +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.cpp new file mode 100644 index 0000000..ab5e041 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.cpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * 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 "ManualVelocitySmoothingZ.hpp" + +#include +#include + +void ManualVelocitySmoothingZ::reset(float accel, float vel, float pos) +{ + _trajectory.reset(accel, vel, pos); + + resetPositionLock(); +} + +void ManualVelocitySmoothingZ::resetPositionLock() +{ + _position_lock_active = false; + _position_setpoint_locked = NAN; +} + +void ManualVelocitySmoothingZ::update(float dt, float velocity_target) +{ + // Update state + updateTrajectories(dt); + + // Set max accel/vel/jerk + // Has to be done before _updateTrajDurations() + updateTrajConstraints(velocity_target); + + // Lock or unlock position + // Has to be done before _updateTrajDurations() + checkPositionLock(velocity_target); + + // Update durations + _trajectory.updateDurations(velocity_target); +} + +void ManualVelocitySmoothingZ::updateTrajectories(float dt) +{ + _trajectory.updateTraj(dt); + + _state.j = _trajectory.getCurrentJerk(); + _state.a = _trajectory.getCurrentAcceleration(); + _state.v = _trajectory.getCurrentVelocity(); + _state.x = _trajectory.getCurrentPosition(); +} + +void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target) +{ + if (velocity_target < 0.f) { // up + _trajectory.setMaxAccel(_max_accel_up); + _trajectory.setMaxVel(_max_vel_up); + + } else { // down + _trajectory.setMaxAccel(_max_accel_down); + _trajectory.setMaxVel(_max_vel_down); + } +} + +void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target) +{ + /** + * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint + * is continuous. We know that the output of the position loop (part of the velocity setpoint) + * will suddenly become null + * and only the feedforward (generated by this flight task) will remain. + * This is why the previous input of the velocity controller + * is used to set current velocity of the trajectory. + */ + if (fabsf(_state.v) < 0.1f && + fabsf(_state.a) < .2f && + fabsf(velocity_target) <= FLT_EPSILON) { + // Lock position + _position_lock_active = true; + _position_setpoint_locked = _state.x; + + } else { + // Unlock position + if (_position_lock_active) { + // Start the trajectory at the current velocity setpoint + _trajectory.setCurrentVelocity(_velocity_setpoint_feedback); + _state.v = _velocity_setpoint_feedback; + resetPositionLock(); + } + + _trajectory.setCurrentPosition(_position_estimate); + } +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.hpp new file mode 100644 index 0000000..3a293cb --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 ManualVelocitySmoothingZ.hpp + * + */ + +#pragma once + +#include + +class ManualVelocitySmoothingZ final +{ +public: + ManualVelocitySmoothingZ() = default; + ~ManualVelocitySmoothingZ() = default; + + void reset(float accel, float vel, float pos); + void update(float dt, float velocity_target); + + void setVelSpFeedback(const float fb) { _velocity_setpoint_feedback = fb; } + + void setMaxJerk(const float max_jerk) + { + _trajectory.setMaxJerk(max_jerk); + } + void setMaxAccelUp(const float max_accel_up) + { + _max_accel_up = max_accel_up; + } + void setMaxAccelDown(const float max_accel_down) + { + _max_accel_down = max_accel_down; + } + void setMaxVelUp(const float max_vel_up) + { + _max_vel_up = max_vel_up; + } + void setMaxVelDown(const float max_vel_down) + { + _max_vel_down = max_vel_down; + } + + float getCurrentJerk() const { return _state.j; } + float getCurrentAcceleration() const { return _state.a; } + void setCurrentVelocity(const float vel) + { + _state.v = vel; + _trajectory.setCurrentVelocity(vel); + } + float getCurrentVelocity() const { return _state.v; } + void setCurrentPosition(const float pos) + { + _state.x = pos; + _trajectory.setCurrentPosition(pos); + _position_estimate = pos; + + if (_position_lock_active) { + _position_setpoint_locked = pos; + } + } + float getCurrentPosition() const { return _position_setpoint_locked; } + void setCurrentPositionEstimate(float pos) { _position_estimate = pos; } + +private: + void resetPositionLock(); + void updateTrajectories(float dt); + void updateTrajConstraints(float vel_target); + void checkPositionLock(float velocity_target); + void updateTrajDurations(float velocity_target); + + VelocitySmoothing _trajectory; ///< Trajectory in z direction + + bool _position_lock_active{false}; + + float _position_setpoint_locked{}; + + float _velocity_setpoint_feedback{}; + float _position_estimate{}; + + struct { + float j; + float a; + float v; + float x; + } _state{}; + + float _max_accel_up{0.f}; + float _max_accel_down{0.f}; + float _max_vel_up{0.f}; + float _max_vel_down{0.f}; +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp new file mode 100644 index 0000000..d9c056a --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * 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 spec{fic 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 StickAccelerationXY.cpp + */ + +#include "StickAccelerationXY.hpp" + +#include +#include "Sticks.hpp" + +using namespace matrix; + +StickAccelerationXY::StickAccelerationXY(ModuleParams *parent) : + ModuleParams(parent) +{ + _brake_boost_filter.reset(1.f); + resetPosition(); +} + +void StickAccelerationXY::resetPosition() +{ + _position_setpoint.setNaN(); +} + +void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity) +{ + _velocity_setpoint = velocity; +} + +void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration) +{ + _acceleration_slew_rate_x.setForcedValue(acceleration(0)); + _acceleration_slew_rate_y.setForcedValue(acceleration(1)); +} + +void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos, + const matrix::Vector2f &vel_sp_feedback, const float dt) +{ + // maximum commanded acceleration and velocity + Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get()); + Vector2f velocity_scale(_param_mpc_vel_manual.get(), _param_mpc_vel_manual.get()); + + acceleration_scale *= 2.f; // because of drag the average acceleration is half + + // Map stick input to acceleration + Sticks::limitStickUnitLengthXY(stick_xy); + Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp); + _acceleration_setpoint = stick_xy.emult(acceleration_scale); + applyJerkLimit(dt); + + // Add drag to limit speed and brake again + Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint); + + // Don't allow the drag to change the sign of the velocity, otherwise we might get into oscillations around 0, due + // to discretization + if (_acceleration_setpoint.norm_squared() < FLT_EPSILON + && _velocity_setpoint.norm_squared() < drag.norm_squared() * dt * dt) { + drag.setZero(); + _velocity_setpoint.setZero(); + } + + _acceleration_setpoint -= drag; + + applyTiltLimit(_acceleration_setpoint); + + // Generate velocity setpoint by forward integrating commanded acceleration + _velocity_setpoint += _acceleration_setpoint * dt; + + lockPosition(pos, vel_sp_feedback, dt); + _acceleration_setpoint_prev = _acceleration_setpoint; +} + +void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp) +{ + pos_sp.xy() = _position_setpoint; + vel_sp.xy() = _velocity_setpoint; + acc_sp.xy() = _acceleration_setpoint; +} + +void StickAccelerationXY::applyJerkLimit(const float dt) +{ + // Apply jerk limit - acceleration slew rate + // Scale each jerk limit with the normalized projection of the acceleration + // setpoint increment to produce a synchronized motion + const Vector2f dir = Vector2f(_acceleration_setpoint - _acceleration_setpoint_prev).unit_or_zero(); + const float jerk_max_x = fabsf(dir(0)) * _param_mpc_jerk_max.get(); + const float jerk_max_y = fabsf(dir(1)) * _param_mpc_jerk_max.get(); + _acceleration_slew_rate_x.setSlewRate(jerk_max_x); + _acceleration_slew_rate_y.setSlewRate(jerk_max_y); + _acceleration_setpoint(0) = _acceleration_slew_rate_x.update(_acceleration_setpoint(0), dt); + _acceleration_setpoint(1) = _acceleration_slew_rate_y.update(_acceleration_setpoint(1), dt); +} + +Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const float dt, const Vector2f &stick_xy, + const Vector2f &vel_sp) +{ + _brake_boost_filter.setParameters(dt, .8f); + + if (stick_xy.norm_squared() < FLT_EPSILON) { + _brake_boost_filter.update(math::max(2.f, sqrtf(_param_mpc_vel_manual.get()))); + + } else { + _brake_boost_filter.update(1.f); + } + + drag_coefficient *= _brake_boost_filter.getState(); + + // increase drag with sqareroot function when velocity is lower than 1m/s + const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm()); + return drag_coefficient.emult(velocity_with_sqrt_boost); +} + +void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration) +{ + // fetch the tilt limit which is lower than the maximum during takeoff + takeoff_status_s takeoff_status{}; + _takeoff_status_sub.copy(&takeoff_status); + + // Check if acceleration would exceed the tilt limit + const float acc = acceleration.length(); + const float acc_tilt_max = tanf(takeoff_status.tilt_limit) * CONSTANTS_ONE_G; + + if (acc > acc_tilt_max) { + acceleration *= acc_tilt_max / acc; + } +} + +void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt) +{ + const bool moving = _velocity_setpoint.norm_squared() > FLT_EPSILON; + const bool position_locked = PX4_ISFINITE(_position_setpoint(0)) || PX4_ISFINITE(_position_setpoint(1)); + + // lock position + if (!moving && !position_locked) { + _position_setpoint = pos.xy(); + } + + // open position loop + if (moving && position_locked) { + _position_setpoint.setNaN(); + + // avoid velocity setpoint jump caused by ignoring remaining position error + if (PX4_ISFINITE(vel_sp_feedback(0)) && PX4_ISFINITE(vel_sp_feedback(1))) { + _velocity_setpoint = vel_sp_feedback; + } + } +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp new file mode 100644 index 0000000..06ae583 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 StickAccelerationXY.hpp + * @brief Generate horizontal position, velocity and acceleration from stick input + * @author Matthias Grob + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "SlewRate.hpp" + +class StickAccelerationXY : public ModuleParams +{ +public: + StickAccelerationXY(ModuleParams *parent); + ~StickAccelerationXY() = default; + + void resetPosition(); + void resetVelocity(const matrix::Vector2f &velocity); + void resetAcceleration(const matrix::Vector2f &acceleration); + void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos, + const matrix::Vector2f &vel_sp_feedback, const float dt); + void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp); + +private: + void applyJerkLimit(const float dt); + matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy, + const matrix::Vector2f &vel_sp); + void applyTiltLimit(matrix::Vector2f &acceleration); + void lockPosition(const matrix::Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt); + + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + + SlewRate _acceleration_slew_rate_x; + SlewRate _acceleration_slew_rate_y; + AlphaFilter _brake_boost_filter; + + matrix::Vector2f _position_setpoint; + matrix::Vector2f _velocity_setpoint; + matrix::Vector2f _acceleration_setpoint; + matrix::Vector2f _acceleration_setpoint_prev; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_tiltmax_air + ) +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp new file mode 100644 index 0000000..5e02170 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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 StickYaw.cpp + */ + +#include "StickYaw.hpp" + +#include + +void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, + const float yaw, const float deltatime) +{ + _yawspeed_slew_rate.setSlewRate(2.f * M_PI_F); + yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime); + yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint); +} + +float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint) +{ + // Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN. + if (fabsf(yawspeed_setpoint) > FLT_EPSILON) { + // no fixed heading when rotating around yaw by stick + return NAN; + + } else { + // break down and hold the current heading when no more rotation commanded + if (!PX4_ISFINITE(yaw_setpoint)) { + return yaw; + + } else { + return yaw_setpoint; + } + } +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp new file mode 100644 index 0000000..3fde9aa --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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 StickYaw.hpp + * @brief Generate yaw and angular yawspeed setpoints from stick input + * @author Matthias Grob + */ + +#pragma once + +#include "SlewRate.hpp" + +class StickYaw +{ +public: + StickYaw() = default; + ~StickYaw() = default; + + void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw, + const float deltatime); + +private: + SlewRate _yawspeed_slew_rate; + + /** + * Lock yaw when not currently turning + * When applying a yawspeed the vehicle is turning, when the speed is + * set to zero the vehicle needs to slow down and then lock at the yaw + * it stops at to not drift over time. + * @param yawspeed current yaw rotational rate state + * @param yaw current yaw rotational rate state + * @param yawspeed_setpoint rotation rate at which to turn around yaw axis + * @param yaw current yaw setpoint which then will be overwritten by the return value + * @return yaw setpoint to execute to have a yaw lock at the correct moment in time + */ + static float updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint); +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp new file mode 100644 index 0000000..a58baf9 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 Sticks.cpp + */ + +#include "Sticks.hpp" + +using namespace time_literals; +using namespace matrix; + +Sticks::Sticks(ModuleParams *parent) : + ModuleParams(parent) +{}; + +bool Sticks::checkAndSetStickInputs() +{ + // Sticks are rescaled linearly and exponentially to [-1,1] + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + // Linear scale + _positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1] + _positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1] + _positions(2) = -(math::constrain(manual_control_setpoint.z, 0.0f, + 1.0f) - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1] + _positions(3) = manual_control_setpoint.r; // yaw [-1,1] + + // Exponential scale + _positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); + _positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); + _positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get()); + _positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get()); + + // valid stick inputs are required + const bool valid_sticks = PX4_ISFINITE(_positions(0)) + && PX4_ISFINITE(_positions(1)) + && PX4_ISFINITE(_positions(2)) + && PX4_ISFINITE(_positions(3)); + + _input_available = valid_sticks; + + } else { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + if (vehicle_status.rc_signal_lost) { + _input_available = false; + } + } + } + + if (!_input_available) { + // Timeout: set all sticks to zero + _positions.zero(); + _positions_expo.zero(); + } + + return _input_available; +} + +void Sticks::limitStickUnitLengthXY(Vector2f &v) +{ + const float vl = v.length(); + + if (vl > 1.0f) { + v /= vl; + } +} + +void Sticks::rotateIntoHeadingFrameXY(Vector2f &v, const float yaw, const float yaw_setpoint) +{ + Vector3f v3(v(0), v(1), 0.f); + const float yaw_rotate = PX4_ISFINITE(yaw_setpoint) ? yaw_setpoint : yaw; + v = Vector2f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * v3); +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp new file mode 100644 index 0000000..dae247f --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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 Sticks.hpp + * + * Library abstracting stick input from manual_control_setpoint + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include +#include +#include +#include + +class Sticks : public ModuleParams +{ +public: + Sticks(ModuleParams *parent); + ~Sticks() = default; + + bool checkAndSetStickInputs(); + bool isAvailable() { return _input_available; }; + const matrix::Vector &getPosition() { return _positions; }; + const matrix::Vector &getPositionExpo() { return _positions_expo; }; + + + /** + * Limit the the horizontal input from a square shaped joystick gimbal to a unit circle + * @param v Vector containing x, y, z axis of the joystick gimbal. x, y get adjusted + */ + static void limitStickUnitLengthXY(matrix::Vector2f &v); + + /** + * Rotate horizontal component of joystick input into the vehicle body orientation + * @param v Vector containing x, y, z axis of the joystick input. + * @param yaw Current vehicle yaw heading + * @param yaw_setpoint Current yaw setpoint if it's locked else NAN + */ + static void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint); + +private: + bool _input_available{false}; + matrix::Vector _positions; ///< unmodified manual stick inputs + matrix::Vector _positions_expo; ///< modified manual sticks using expo function + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_hold_dz, + (ParamFloat) _param_mpc_xy_man_expo, + (ParamFloat) _param_mpc_z_man_expo, + (ParamFloat) _param_mpc_yaw_expo + ) +}; diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StraightLine.cpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StraightLine.cpp new file mode 100644 index 0000000..eb49726 --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StraightLine.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 StraightLine.cpp + */ + +#include "StraightLine.hpp" +#include +#include + +using namespace matrix; + +void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint) +{ + if (isEndReached()) { + // Vehicle has reached target, lock position + position_setpoint = _end; + velocity_setpoint.setNaN(); + return; + } + + Vector3f start_to_end = _end - _start; + float distance_start_to_end = start_to_end.norm(); + + // capture progress as ratio between 0 and 1 of entire distance + Vector3f vehicle_to_end = _end - _position; + float distance_vehicle_to_end = vehicle_to_end.norm(); + float distance_from_start = Vector3f(_start - _position).norm(); + float progress = distance_from_start / (distance_from_start + distance_vehicle_to_end); + + float distance_from_boundary = 0.f; + + // calculate the distance to the closer boundary + if (progress < 0.5f) { + distance_from_boundary = (2 * progress) * distance_start_to_end; + + } else { + distance_from_boundary = (2 * (1 - progress)) * distance_start_to_end; + } + + // ramp velocity based on the distance to the boundary + float velocity = 0.5f + (distance_from_boundary / 4.f); + velocity = math::min(velocity, _speed); + velocity_setpoint = vehicle_to_end.unit_or_zero() * velocity; + + // check if we plan to go against the line direction which indicates we reached the goal + if (start_to_end * vehicle_to_end < 0) { + _end_reached = true; + } +} + +void StraightLine::setLineFromTo(const Vector3f &start, const Vector3f &end) +{ + if (PX4_ISFINITE(start.norm_squared()) && PX4_ISFINITE(end.norm_squared())) { + _start = start; + _end = end; + _end_reached = false; + } +} diff --git a/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StraightLine.hpp b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StraightLine.hpp new file mode 100644 index 0000000..cdd776d --- /dev/null +++ b/src/prometheus_px4/src/modules/flight_mode_manager/tasks/Utility/StraightLine.hpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 StraightLine.hpp + * + * lib to return setpoints on a straight line + * + * @author Christoph Tobler + */ + +#pragma once + +#include + +class StraightLine +{ +public: + StraightLine(const matrix::Vector3f &pos) : _position(pos) {}; + ~StraightLine() = default; + + // setter functions + void setLineFromTo(const matrix::Vector3f &start, const matrix::Vector3f &end); + void setSpeed(const float &speed) { _speed = speed; }; + + /** + * Generate setpoints on a straight line according to parameters + * + * @param position_setpoint: reference to the 3D vector with the position setpoint to update + * @param velocity_setpoint: reference to the 3D vector with the velocity setpoint to update + */ + void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint); + + /** + * Check if the end was reached + * + * @return false when on the way from start to end, true when end was reached + */ + bool isEndReached() const { return _end_reached; } + + void reset() { _end_reached = true; } + +private: + const matrix::Vector3f &_position; /**< vehicle position (dependency injection) */ + + matrix::Vector3f _start; /**< Start point of the straight line */ + matrix::Vector3f _end; /**< End point of the straight line */ + float _speed = 1.f; /**< desired speed between accelerating and decelerating */ + + bool _end_reached = true; /**< Flag to lock further movement when end is reached */ +}; diff --git a/src/prometheus_px4/src/modules/fw_att_control/CMakeLists.txt b/src/prometheus_px4/src/modules/fw_att_control/CMakeLists.txt new file mode 100644 index 0000000..a8c79cf --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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__fw_att_control + MAIN fw_att_control + SRCS + FixedwingAttitudeControl.cpp + FixedwingAttitudeControl.hpp + + ecl_controller.cpp + ecl_pitch_controller.cpp + ecl_roll_controller.cpp + ecl_wheel_controller.cpp + ecl_yaw_controller.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/prometheus_px4/src/modules/fw_att_control/FixedwingAttitudeControl.cpp new file mode 100644 index 0000000..65086c0 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -0,0 +1,775 @@ +/**************************************************************************** + * + * 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 "FixedwingAttitudeControl.hpp" + +#include + +using namespace time_literals; +using math::constrain; +using math::gradual; +using math::radians; + +FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)), + _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + // check if VTOL first + if (vtol) { + int32_t vt_type = -1; + + if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { + _is_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + } + } + + /* fetch initial parameter values */ + parameters_update(); + + // set initial maximum body rate setpoints + _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get())); + _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); + _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); + _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); +} + +FixedwingAttitudeControl::~FixedwingAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +FixedwingAttitudeControl::init() +{ + if (!_att_sub.registerCallback()) { + PX4_ERR("vehicle attitude callback registration failed!"); + return false; + } + + return true; +} + +int +FixedwingAttitudeControl::parameters_update() +{ + /* pitch control parameters */ + _pitch_ctrl.set_time_constant(_param_fw_p_tc.get()); + _pitch_ctrl.set_k_p(_param_fw_pr_p.get()); + _pitch_ctrl.set_k_i(_param_fw_pr_i.get()); + _pitch_ctrl.set_k_ff(_param_fw_pr_ff.get()); + _pitch_ctrl.set_integrator_max(_param_fw_pr_imax.get()); + + /* roll control parameters */ + _roll_ctrl.set_time_constant(_param_fw_r_tc.get()); + _roll_ctrl.set_k_p(_param_fw_rr_p.get()); + _roll_ctrl.set_k_i(_param_fw_rr_i.get()); + _roll_ctrl.set_k_ff(_param_fw_rr_ff.get()); + _roll_ctrl.set_integrator_max(_param_fw_rr_imax.get()); + + /* yaw control parameters */ + _yaw_ctrl.set_k_p(_param_fw_yr_p.get()); + _yaw_ctrl.set_k_i(_param_fw_yr_i.get()); + _yaw_ctrl.set_k_ff(_param_fw_yr_ff.get()); + _yaw_ctrl.set_integrator_max(_param_fw_yr_imax.get()); + + /* wheel control parameters */ + _wheel_ctrl.set_k_p(_param_fw_wr_p.get()); + _wheel_ctrl.set_k_i(_param_fw_wr_i.get()); + _wheel_ctrl.set_k_ff(_param_fw_wr_ff.get()); + _wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get()); + _wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get())); + + return PX4_OK; +} + +void +FixedwingAttitudeControl::vehicle_control_mode_poll() +{ + _vcontrol_mode_sub.update(&_vcontrol_mode); + + if (_vehicle_status.is_vtol) { + const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode; + const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; + + if (is_hovering || is_tailsitter_transition) { + _vcontrol_mode.flag_control_attitude_enabled = false; + _vcontrol_mode.flag_control_manual_enabled = false; + } + } +} + +void +FixedwingAttitudeControl::vehicle_manual_poll() +{ + const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode; + const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) { + + // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + + if (!_vcontrol_mode.flag_control_climb_rate_enabled) { + + if (_vcontrol_mode.flag_control_attitude_enabled) { + // STABILIZED mode generate the attitude setpoint from manual user inputs + + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + + _att_sp.pitch_body = -_manual_control_setpoint.x * radians(_param_fw_man_p_max.get()) + + radians(_param_fw_psp_off.get()); + _att_sp.pitch_body = constrain(_att_sp.pitch_body, + -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); + + _att_sp.yaw_body = 0.0f; + _att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + + Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + q.copyTo(_att_sp.q_d); + + _att_sp.timestamp = hrt_absolute_time(); + + _attitude_sp_pub.publish(_att_sp); + + } else if (_vcontrol_mode.flag_control_rates_enabled && + !_vcontrol_mode.flag_control_attitude_enabled) { + + // RATE mode we need to generate the rate setpoint from manual user inputs + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get()); + _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get()); + _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()); + _rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + + _rate_sp_pub.publish(_rates_sp); + + } else { + /* manual/direct control */ + _actuators.control[actuator_controls_s::INDEX_ROLL] = + _manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _actuators.control[actuator_controls_s::INDEX_PITCH] = + -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); + _actuators.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + } + } + } + } +} + +void +FixedwingAttitudeControl::vehicle_attitude_setpoint_poll() +{ + if (_att_sp_sub.update(&_att_sp)) { + _rates_sp.thrust_body[0] = _att_sp.thrust_body[0]; + _rates_sp.thrust_body[1] = _att_sp.thrust_body[1]; + _rates_sp.thrust_body[2] = _att_sp.thrust_body[2]; + } +} + +void +FixedwingAttitudeControl::vehicle_rates_setpoint_poll() +{ + if (_rates_sp_sub.update(&_rates_sp)) { + if (_is_tailsitter) { + float tmp = _rates_sp.roll; + _rates_sp.roll = -_rates_sp.yaw; + _rates_sp.yaw = tmp; + } + } +} + +void +FixedwingAttitudeControl::vehicle_land_detected_poll() +{ + 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; + } + } +} + +float FixedwingAttitudeControl::get_airspeed_and_update_scaling() +{ + _airspeed_validated_sub.update(); + const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s) + && (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s); + + // if no airspeed measurement is available out best guess is to use the trim airspeed + float airspeed = _param_fw_airspd_trim.get(); + + if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s); + + } else { + // VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible + // this assumption is good as long as the vehicle is not hovering in a headwind which is much larger + // than the stall airspeed + if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode) { + airspeed = _param_fw_airspd_stall.get(); + } + } + + /* + * For scaling our actuators using anything less than the stall + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(), + _param_fw_airspd_max.get()), 0.1f, 1000.0f); + + _airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f; + + return airspeed; +} + +void FixedwingAttitudeControl::Run() +{ + if (should_exit()) { + _att_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // only run controller if attitude changed + vehicle_attitude_s att; + + if (_att_sub.update(&att)) { + + // only update parameters if they changed + bool params_updated = _parameter_update_sub.updated(); + + // check for parameter updates + if (params_updated) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + parameters_update(); + } + + const float dt = math::constrain((att.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f); + _last_run = att.timestamp; + + /* get current rotation matrix and euler angles from control state quaternions */ + matrix::Dcmf R = matrix::Quatf(att.q); + + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_rates_sub.copy(&angular_velocity); + float rollspeed = angular_velocity.xyz[0]; + float pitchspeed = angular_velocity.xyz[1]; + float yawspeed = angular_velocity.xyz[2]; + + if (_is_tailsitter) { + /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode + * + * Since the VTOL airframe is initialized as a multicopter we need to + * modify the estimated attitude for the fixed wing operation. + * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around + * the pitch axis compared to the neutral position of the vehicle in multicopter mode + * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. + * Additionally, in order to get the correct sign of the pitch, we need to multiply + * the new x axis of the rotation matrix with -1 + * + * original: modified: + * + * Rxx Ryx Rzx -Rzx Ryx Rxx + * Rxy Ryy Rzy -Rzy Ryy Rxy + * Rxz Ryz Rzz -Rzz Ryz Rxz + * */ + matrix::Dcmf R_adapted = R; //modified rotation matrix + + /* move z to x */ + R_adapted(0, 0) = R(0, 2); + R_adapted(1, 0) = R(1, 2); + R_adapted(2, 0) = R(2, 2); + + /* move x to z */ + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + /* change direction of pitch (convert to right handed system) */ + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + + /* fill in new attitude data */ + R = R_adapted; + + /* lastly, roll- and yawspeed have to be swaped */ + float helper = rollspeed; + rollspeed = -yawspeed; + yawspeed = helper; + } + + const matrix::Eulerf euler_angles(R); + + vehicle_attitude_setpoint_poll(); + + // vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition + _vehicle_status_sub.update(&_vehicle_status); + + vehicle_control_mode_poll(); + vehicle_manual_poll(); + vehicle_land_detected_poll(); + + // the position controller will not emit attitude setpoints in some modes + // we need to make sure that this flag is reset + _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + + bool wheel_control = false; + + // TODO: manual wheel_control on ground? + if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) { + wheel_control = true; + } + + // lock integrator if no rate control enabled, or in RW mode (but not transitioning VTOL or tailsitter), or for long intervals (> 20 ms) + bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled + || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_vehicle_status.in_transition_mode && !_is_tailsitter) + || (dt > 0.02f); + + /* if we are in rotary wing mode, do nothing */ + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { + perf_end(_loop_perf); + return; + } + + control_flaps(dt); + + /* decide if in stabilized or full manual control */ + if (_vcontrol_mode.flag_control_rates_enabled) { + + const float airspeed = get_airspeed_and_update_scaling(); + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); + } + + /* Reset integrators if the aircraft is on ground + * or a multicopter (but not transitioning VTOL or tailsitter) + */ + if (_landed + || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode && !_is_tailsitter)) { + + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); + } + + /* Prepare data for attitude controllers */ + ECL_ControlData control_input{}; + control_input.roll = euler_angles.phi(); + control_input.pitch = euler_angles.theta(); + control_input.yaw = euler_angles.psi(); + control_input.body_x_rate = rollspeed; + control_input.body_y_rate = pitchspeed; + control_input.body_z_rate = yawspeed; + control_input.roll_setpoint = _att_sp.roll_body; + control_input.pitch_setpoint = _att_sp.pitch_body; + control_input.yaw_setpoint = _att_sp.yaw_body; + control_input.airspeed_min = _param_fw_airspd_stall.get(); + control_input.airspeed_max = _param_fw_airspd_max.get(); + control_input.airspeed = airspeed; + control_input.scaler = _airspeed_scaling; + control_input.lock_integrator = lock_integrator; + + if (wheel_control) { + _local_pos_sub.update(&_local_pos); + + /* Use stall airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); + float gspd_scaling_trim = (_param_fw_airspd_stall.get()); + + control_input.groundspeed = groundspeed; + + if (groundspeed > gspd_scaling_trim) { + control_input.groundspeed_scaler = gspd_scaling_trim / groundspeed; + + } else { + control_input.groundspeed_scaler = 1.0f; + } + } + + /* reset body angular rate limits on mode change */ + if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { + if (_vcontrol_mode.flag_control_attitude_enabled + || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get())); + _pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get())); + _pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get())); + _yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get())); + + } else { + _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get())); + _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); + _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); + _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); + } + } + + _flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled; + + /* bi-linear interpolation over airspeed for actuator trim scheduling */ + float trim_roll = _param_trim_roll.get(); + float trim_pitch = _param_trim_pitch.get(); + float trim_yaw = _param_trim_yaw.get(); + + if (airspeed < _param_fw_airspd_trim.get()) { + trim_roll += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim_pitch += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim_yaw += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(), + 0.0f); + + } else { + trim_roll += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_r_vmax.get()); + trim_pitch += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_p_vmax.get()); + trim_yaw += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_y_vmax.get()); + } + + /* add trim increment if flaps are deployed */ + trim_roll += _flaps_applied * _param_fw_dtrim_r_flps.get(); + trim_pitch += _flaps_applied * _param_fw_dtrim_p_flps.get(); + + /* Run attitude controllers */ + if (_vcontrol_mode.flag_control_attitude_enabled) { + if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { + _roll_ctrl.control_attitude(dt, control_input); + _pitch_ctrl.control_attitude(dt, control_input); + + if (wheel_control) { + _wheel_ctrl.control_attitude(dt, control_input); + _yaw_ctrl.reset_integrator(); + + } else { + // runs last, because is depending on output of roll and pitch attitude + _yaw_ctrl.control_attitude(dt, control_input); + _wheel_ctrl.reset_integrator(); + } + + /* Update input data for rate controllers */ + control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); + control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); + control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + float roll_u = _roll_ctrl.control_euler_rate(dt, control_input); + _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; + + if (!PX4_ISFINITE(roll_u)) { + _roll_ctrl.reset_integrator(); + } + + float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input); + _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; + + if (!PX4_ISFINITE(pitch_u)) { + _pitch_ctrl.reset_integrator(); + } + + float yaw_u = 0.0f; + + if (wheel_control) { + yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input); + + } else { + yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input); + } + + _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; + + /* add in manual rudder control in manual modes */ + if (_vcontrol_mode.flag_control_manual_enabled) { + _actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r; + } + + if (!PX4_ISFINITE(yaw_u)) { + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); + } + + /* throttle passed through if it is finite and if no engine failure was detected */ + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0]) + && !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f; + + /* scale effort by battery status */ + if (_param_fw_bat_scale_en.get() && + _actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { + + if (_battery_status_sub.updated()) { + battery_status_s battery_status{}; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_scale = battery_status.scale; + } + } + + _actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale; + } + } + + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + _rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); + _rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); + _rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); + + _rates_sp.timestamp = hrt_absolute_time(); + + _rate_sp_pub.publish(_rates_sp); + + } else { + vehicle_rates_setpoint_poll(); + + _roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll); + _yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw); + _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch); + + float roll_u = _roll_ctrl.control_bodyrate(dt, control_input); + _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; + + float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input); + _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; + + float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input); + _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; + + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? + _rates_sp.thrust_body[0] : 0.0f; + } + + rate_ctrl_status_s rate_ctrl_status{}; + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); + rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); + + if (wheel_control) { + rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); + + } else { + rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); + } + + _rate_ctrl_status_pub.publish(rate_ctrl_status); + } + + // Add feed-forward from roll control output to yaw control output + // This can be used to counteract the adverse yaw effect when rolling the plane + _actuators.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get() + * constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); + + _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; + _actuators.control[5] = _manual_control_setpoint.aux1; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; + // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future + _actuators.control[7] = _manual_control_setpoint.aux3; + + /* lazily publish the setpoint only once available */ + _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = att.timestamp; + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { + _actuators_0_pub.publish(_actuators); + } + } + + perf_end(_loop_perf); +} + +void FixedwingAttitudeControl::control_flaps(const float dt) +{ + /* default flaps to center */ + float flap_control = 0.0f; + + /* map flaps by default to manual if valid */ + if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled + && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { + flap_control = 0.5f * (_manual_control_setpoint.flaps + 1.0f) * _param_fw_flaps_scl.get(); + + } else if (_vcontrol_mode.flag_control_auto_enabled + && fabsf(_param_fw_flaps_scl.get()) > 0.01f) { + + switch (_att_sp.apply_flaps) { + case vehicle_attitude_setpoint_s::FLAPS_OFF: + flap_control = 0.0f; // no flaps + break; + + case vehicle_attitude_setpoint_s::FLAPS_LAND: + flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_lnd_scl.get(); + break; + + case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF: + flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_to_scl.get(); + break; + } + } + + // move the actual control value continuous with time, full flap travel in 1sec + if (fabsf(_flaps_applied - flap_control) > 0.01f) { + _flaps_applied += (_flaps_applied - flap_control) < 0 ? dt : -dt; + + } else { + _flaps_applied = flap_control; + } + + /* default flaperon to center */ + float flaperon_control = 0.0f; + + /* map flaperons by default to manual if valid */ + if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled + && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { + + flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get(); + + } else if (_vcontrol_mode.flag_control_auto_enabled + && fabsf(_param_fw_flaperon_scl.get()) > 0.01f) { + + if (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) { + flaperon_control = _param_fw_flaperon_scl.get(); + + } else { + flaperon_control = 0.0f; + } + } + + // move the actual control value continuous with time, full flap travel in 1sec + if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) { + _flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt; + + } else { + _flaperons_applied = flaperon_control; + } +} + +int FixedwingAttitudeControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + FixedwingAttitudeControl *instance = new FixedwingAttitudeControl(vtol); + + 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 FixedwingAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FixedwingAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_att_control is the fixed wing attitude controller. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fw_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]) +{ + return FixedwingAttitudeControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/prometheus_px4/src/modules/fw_att_control/FixedwingAttitudeControl.hpp new file mode 100644 index 0000000..fc5ac68 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * 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 +#include "ecl_pitch_controller.h" +#include "ecl_roll_controller.h" +#include "ecl_wheel_controller.h" +#include "ecl_yaw_controller.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +class FixedwingAttitudeControl final : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + FixedwingAttitudeControl(bool vtol = false); + ~FixedwingAttitudeControl() 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::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */ + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */ + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + + uORB::Publication _actuators_0_pub; + uORB::Publication _attitude_sp_pub; + uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; + + actuator_controls_s _actuators {}; /**< actuator control inputs */ + manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ + vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */ + vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ + vehicle_local_position_s _local_pos {}; /**< local position */ + vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */ + vehicle_status_s _vehicle_status {}; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + hrt_abstime _last_run{0}; + + float _flaps_applied{0.0f}; + float _flaperons_applied{0.0f}; + + float _airspeed_scaling{1.0f}; + + bool _landed{true}; + + float _battery_scale{1.0f}; + + bool _flag_control_attitude_enabled_last{false}; + + bool _is_tailsitter{false}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_fw_acro_x_max, + (ParamFloat) _param_fw_acro_y_max, + (ParamFloat) _param_fw_acro_z_max, + + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_airspd_stall, + (ParamFloat) _param_fw_airspd_trim, + (ParamInt) _param_fw_arsp_mode, + + (ParamInt) _param_fw_arsp_scale_en, + + (ParamBool) _param_fw_bat_scale_en, + + (ParamFloat) _param_fw_dtrim_p_flps, + (ParamFloat) _param_fw_dtrim_p_vmax, + (ParamFloat) _param_fw_dtrim_p_vmin, + (ParamFloat) _param_fw_dtrim_r_flps, + (ParamFloat) _param_fw_dtrim_r_vmax, + (ParamFloat) _param_fw_dtrim_r_vmin, + (ParamFloat) _param_fw_dtrim_y_vmax, + (ParamFloat) _param_fw_dtrim_y_vmin, + + (ParamFloat) _param_fw_flaperon_scl, + (ParamFloat) _param_fw_flaps_lnd_scl, + (ParamFloat) _param_fw_flaps_scl, + (ParamFloat) _param_fw_flaps_to_scl, + + (ParamFloat) _param_fw_man_p_max, + (ParamFloat) _param_fw_man_p_sc, + (ParamFloat) _param_fw_man_r_max, + (ParamFloat) _param_fw_man_r_sc, + (ParamFloat) _param_fw_man_y_sc, + + (ParamFloat) _param_fw_p_rmax_neg, + (ParamFloat) _param_fw_p_rmax_pos, + (ParamFloat) _param_fw_p_tc, + (ParamFloat) _param_fw_pr_ff, + (ParamFloat) _param_fw_pr_i, + (ParamFloat) _param_fw_pr_imax, + (ParamFloat) _param_fw_pr_p, + (ParamFloat) _param_fw_psp_off, + + (ParamFloat) _param_fw_r_rmax, + (ParamFloat) _param_fw_r_tc, + (ParamFloat) _param_fw_rll_to_yaw_ff, + (ParamFloat) _param_fw_rr_ff, + (ParamFloat) _param_fw_rr_i, + (ParamFloat) _param_fw_rr_imax, + (ParamFloat) _param_fw_rr_p, + + (ParamBool) _param_fw_w_en, + (ParamFloat) _param_fw_w_rmax, + (ParamFloat) _param_fw_wr_ff, + (ParamFloat) _param_fw_wr_i, + (ParamFloat) _param_fw_wr_imax, + (ParamFloat) _param_fw_wr_p, + + (ParamFloat) _param_fw_y_rmax, + (ParamFloat) _param_fw_yr_ff, + (ParamFloat) _param_fw_yr_i, + (ParamFloat) _param_fw_yr_imax, + (ParamFloat) _param_fw_yr_p, + + (ParamFloat) _param_trim_pitch, + (ParamFloat) _param_trim_roll, + (ParamFloat) _param_trim_yaw + ) + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + ECL_WheelController _wheel_ctrl; + + void control_flaps(const float dt); + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + void vehicle_control_mode_poll(); + void vehicle_manual_poll(); + void vehicle_attitude_setpoint_poll(); + void vehicle_rates_setpoint_poll(); + void vehicle_land_detected_poll(); + + float get_airspeed_and_update_scaling(); +}; diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_controller.cpp b/src/prometheus_px4/src/modules/fw_att_control/ecl_controller.cpp new file mode 100644 index 0000000..f785319 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_controller.cpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_controller.cpp + * Definition of base class for other controllers + * + * @author Lorenz Meier + * @author Thomas Gubler + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#include "ecl_controller.h" + +#include +#include + +ECL_Controller::ECL_Controller() : + _last_run(0), + _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_ff(0.0f), + _integrator_max(0.0f), + _max_rate(0.0f), + _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), + _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f) +{ +} + +void ECL_Controller::reset_integrator() +{ + _integrator = 0.0f; +} + +void ECL_Controller::set_time_constant(float time_constant) +{ + if (time_constant > 0.1f && time_constant < 3.0f) { + _tc = time_constant; + } +} + +void ECL_Controller::set_k_p(float k_p) +{ + _k_p = k_p; +} + +void ECL_Controller::set_k_i(float k_i) +{ + _k_i = k_i; +} + +void ECL_Controller::set_k_ff(float k_ff) +{ + _k_ff = k_ff; +} + +void ECL_Controller::set_integrator_max(float max) +{ + _integrator_max = max; +} + +void ECL_Controller::set_max_rate(float max_rate) +{ + _max_rate = max_rate; +} + +void ECL_Controller::set_bodyrate_setpoint(float rate) +{ + _bodyrate_setpoint = math::constrain(rate, -_max_rate, _max_rate); +} + +float ECL_Controller::get_rate_error() +{ + return _rate_error; +} + +float ECL_Controller::get_desired_rate() +{ + return _rate_setpoint; +} + +float ECL_Controller::get_desired_bodyrate() +{ + return _bodyrate_setpoint; +} + +float ECL_Controller::get_integrator() +{ + return _integrator; +} + +float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) +{ + float airspeed_result = airspeed; + + if (!PX4_ISFINITE(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed_result = 0.5f * (minspeed + maxspeed); + + } else if (airspeed < minspeed) { + airspeed_result = minspeed; + } + + return airspeed_result; +} diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_controller.h b/src/prometheus_px4/src/modules/fw_att_control/ecl_controller.h new file mode 100644 index 0000000..37fd64e --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_controller.h @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_controller.h + * Definition of base class for other controllers + * + * @author Lorenz Meier + * @author Thomas Gubler + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#pragma once + +#include +#include + +struct ECL_ControlData { + float roll; + float pitch; + float yaw; + float body_x_rate; + float body_y_rate; + float body_z_rate; + float roll_setpoint; + float pitch_setpoint; + float yaw_setpoint; + float roll_rate_setpoint; + float pitch_rate_setpoint; + float yaw_rate_setpoint; + float airspeed_min; + float airspeed_max; + float airspeed; + float scaler; + float groundspeed; + float groundspeed_scaler; + bool lock_integrator; +}; + +class ECL_Controller +{ +public: + ECL_Controller(); + virtual ~ECL_Controller() = default; + + virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0; + virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) = 0; + virtual float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) = 0; + + /* Setters */ + void set_time_constant(float time_constant); + void set_k_p(float k_p); + void set_k_i(float k_i); + void set_k_ff(float k_ff); + void set_integrator_max(float max); + void set_max_rate(float max_rate); + void set_bodyrate_setpoint(float rate); + + /* Getters */ + float get_rate_error(); + float get_desired_rate(); + float get_desired_bodyrate(); + float get_integrator(); + + void reset_integrator(); + +protected: + uint64_t _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_ff; + float _integrator_max; + float _max_rate; + float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _bodyrate_setpoint; + float constrain_airspeed(float airspeed, float minspeed, float maxspeed); +}; diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/prometheus_px4/src/modules/fw_att_control/ecl_pitch_controller.cpp new file mode 100644 index 0000000..47890ef --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp + * Implementation of a simple orthogonal pitch PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_pitch_controller.h" +#include +#include +#include + +float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && + PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.airspeed))) { + + return _rate_setpoint; + } + + /* Calculate the error */ + float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = pitch_error / _tc; + + return _rate_setpoint; +} + +float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.body_y_rate) && + PX4_ISFINITE(ctl_data.body_z_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { + + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.body_y_rate; + + if (!ctl_data.lock_integrator && _k_i > 0.0f) { + + /* Integral term scales with 1/IAS^2 */ + float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + /* add and constrain */ + _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); + } + + /* Apply PI rate controller and store non-limited output */ + /* FF terms scales with 1/TAS and P,I with 1/IAS^2 */ + _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + + _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + + _integrator; + + return math::constrain(_last_output, -1.0f, 1.0f); +} + +float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data) +{ + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; + + set_bodyrate_setpoint(_bodyrate_setpoint); + + return control_bodyrate(dt, ctl_data); +} diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_pitch_controller.h b/src/prometheus_px4/src/modules/fw_att_control/ecl_pitch_controller.h new file mode 100644 index 0000000..fd09563 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_pitch_controller.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.h + * Definition of a simple orthogonal pitch PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#ifndef ECL_PITCH_CONTROLLER_H +#define ECL_PITCH_CONTROLLER_H + +#include + +#include "ecl_controller.h" + +class ECL_PitchController : + public ECL_Controller +{ +public: + ECL_PitchController() = default; + ~ECL_PitchController() = default; + + float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; + float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override; + float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; + + /* Additional Setters */ + void set_max_rate_pos(float max_rate_pos) + { + _max_rate = max_rate_pos; + } + + void set_max_rate_neg(float max_rate_neg) + { + _max_rate_neg = max_rate_neg; + } + + void set_bodyrate_setpoint(float rate) + { + _bodyrate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate); + } + +protected: + float _max_rate_neg{0.0f}; +}; + +#endif // ECL_PITCH_CONTROLLER_H diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/prometheus_px4/src/modules/fw_att_control/ecl_roll_controller.cpp new file mode 100644 index 0000000..9bc44b7 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_roll_controller.cpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.cpp + * Implementation of a simple orthogonal roll PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_roll_controller.h" +#include +#include +#include + +float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && + PX4_ISFINITE(ctl_data.roll))) { + + return _rate_setpoint; + } + + /* Calculate the error */ + float roll_error = ctl_data.roll_setpoint - ctl_data.roll; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = roll_error / _tc; + + return _rate_setpoint; +} + +float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.body_x_rate) && + PX4_ISFINITE(ctl_data.body_z_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { + + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; + + if (!ctl_data.lock_integrator && _k_i > 0.0f) { + + /* Integral term scales with 1/IAS^2 */ + float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + /* add and constrain */ + _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); + } + + /* Apply PI rate controller and store non-limited output */ + /* FF terms scales with 1/TAS and P,I with 1/IAS^2 */ + _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + + _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + + _integrator; + + return math::constrain(_last_output, -1.0f, 1.0f); +} + +float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data) +{ + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; + + set_bodyrate_setpoint(_bodyrate_setpoint); + + return control_bodyrate(dt, ctl_data); +} diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_roll_controller.h b/src/prometheus_px4/src/modules/fw_att_control/ecl_roll_controller.h new file mode 100644 index 0000000..f51d349 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_roll_controller.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.h + * Definition of a simple orthogonal roll PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#ifndef ECL_ROLL_CONTROLLER_H +#define ECL_ROLL_CONTROLLER_H + +#include "ecl_controller.h" + +class ECL_RollController : + public ECL_Controller +{ +public: + ECL_RollController() = default; + ~ECL_RollController() = default; + + float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; + float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override; + float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; +}; + +#endif // ECL_ROLL_CONTROLLER_H diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_wheel_controller.cpp b/src/prometheus_px4/src/modules/fw_att_control/ecl_wheel_controller.cpp new file mode 100644 index 0000000..7653e26 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_wheel_controller.cpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2016 Estimation and Control Library (ECL). 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 ECL 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 ecl_wheel_controller.cpp + * Implementation of a simple PID wheel controller for heading tracking. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_wheel_controller.h" +#include +#include +#include +#include + +using matrix::wrap_pi; + +float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.body_z_rate) && + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { + + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* input conditioning */ + float min_speed = 1.0f; + + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error + + if (!ctl_data.lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { + + float id = _rate_error * dt * ctl_data.groundspeed_scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + /* add and constrain */ + _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); + } + + /* Apply PI rate controller and store non-limited output */ + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + _integrator); + + return math::constrain(_last_output, -1.0f, 1.0f); +} + +float ECL_WheelController::control_attitude(const float dt, const ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw))) { + + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw); + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_wheel_controller.h b/src/prometheus_px4/src/modules/fw_att_control/ecl_wheel_controller.h new file mode 100644 index 0000000..f091365 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_wheel_controller.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2016 Estimation and Control Library (ECL). 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 ECL 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 ecl_wheel_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ +#ifndef ECL_HEADING_CONTROLLER_H +#define ECL_HEADING_CONTROLLER_H + +#include "ecl_controller.h" + +class ECL_WheelController : + public ECL_Controller +{ +public: + ECL_WheelController() = default; + ~ECL_WheelController() = default; + + float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; + + float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; + + float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override { (void)ctl_data; return 0; } +}; + +#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/prometheus_px4/src/modules/fw_att_control/ecl_yaw_controller.cpp new file mode 100644 index 0000000..f848866 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_yaw_controller.h" +#include +#include +#include + +float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.roll_rate_setpoint) && + PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { + + return _rate_setpoint; + } + + float constrained_roll; + bool inverted = false; + + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(ctl_data.roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); + + } else { + inverted = true; + + // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity + //note: the ranges are extended by 10 deg here to avoid numeric resolution effects + if (ctl_data.roll > 0.0f) { + /* right hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); + + } else { + /* left hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f)); + } + } + + constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint)); + + + if (!inverted) { + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < + ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); + } + + if (!PX4_ISFINITE(_rate_setpoint)) { + PX4_WARN("yaw rate sepoint not finite"); + _rate_setpoint = 0.0f; + } + + return _rate_setpoint; +} + +float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.body_y_rate) && + PX4_ISFINITE(ctl_data.body_z_rate) && + PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { + + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; + + if (!ctl_data.lock_integrator && _k_i > 0.0f) { + + /* Integral term scales with 1/IAS^2 */ + float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + /* add and constrain */ + _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); + } + + /* Apply PI rate controller and store non-limited output */ + /* FF terms scales with 1/TAS and P,I with 1/IAS^2 */ + _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + + _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + + _integrator; + + return math::constrain(_last_output, -1.0f, 1.0f); +} + +float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data) +{ + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + + set_bodyrate_setpoint(_bodyrate_setpoint); + + return control_bodyrate(dt, ctl_data); +} diff --git a/src/prometheus_px4/src/modules/fw_att_control/ecl_yaw_controller.h b/src/prometheus_px4/src/modules/fw_att_control/ecl_yaw_controller.h new file mode 100644 index 0000000..8d9ab49 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/ecl_yaw_controller.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#ifndef ECL_YAW_CONTROLLER_H +#define ECL_YAW_CONTROLLER_H + +#include "ecl_controller.h" + +class ECL_YawController : + public ECL_Controller +{ +public: + ECL_YawController() = default; + ~ECL_YawController() = default; + + float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; + float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override; + float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; + +protected: + float _max_rate{0.0f}; + +}; + +#endif // ECL_YAW_CONTROLLER_H diff --git a/src/prometheus_px4/src/modules/fw_att_control/fw_att_control_params.c b/src/prometheus_px4/src/modules/fw_att_control/fw_att_control_params.c new file mode 100644 index 0000000..4bc2ad6 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_att_control/fw_att_control_params.c @@ -0,0 +1,737 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 fw_att_control_params.c + * + * Parameters defined by the fixed-wing attitude control task + * + * @author Lorenz Meier + * @author Thomas Gubler + */ + +/* + * Controller parameters, accessible via MAVLink + * + */ + +/** + * Attitude Roll Time Constant + * + * This defines the latency between a roll step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit s + * @min 0.4 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f); + +/** + * Attitude pitch time constant + * + * This defines the latency between a pitch step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit s + * @min 0.2 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f); + +/** + * Pitch rate proportional gain. + * + * This defines how much the elevator input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); + +/** + * Pitch rate integrator gain. + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f); + +/** + * Maximum positive / up pitch rate. + * + * This limits the maximum pitch up angular rate the controller will output (in + * degrees per second). + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); + +/** + * Maximum negative / down pitch rate. + * + * This limits the maximum pitch down up angular rate the controller will + * output (in degrees per second). + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); + +/** + * Pitch rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); + +/** + * Roll rate proportional Gain + * + * This defines how much the aileron input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); + +/** + * Roll rate integrator Gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.2 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f); + +/** + * Roll integrator anti-windup + * + * The portion of the integrator part in the control surface deflection is limited to this value. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); + +/** + * Maximum roll rate + * + * This limits the maximum roll rate the controller will output (in degrees per + * second). + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f); + +/** + * Yaw rate proportional gain + * + * This defines how much the rudder input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); + +/** + * Yaw rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.0 + * @max 50.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_YR_I, 0.1f); + +/** + * Yaw rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); + +/** + * Maximum yaw rate + * + * This limits the maximum yaw rate the controller will output (in degrees per + * second). + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_Y_RMAX, 50.0f); + +/** + * Roll control to yaw control feedforward gain. + * + * This gain can be used to counteract the "adverse yaw" effect for fixed wings. + * When the plane enters a roll it will tend to yaw the nose out of the turn. + * This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract + * this effect. + * + * @min 0.0 + * @decimal 1 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f); + +/** + * Enable wheel steering controller + * + * @boolean + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_W_EN, 0); + + +/** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @unit %/rad/s + * @min 0.005 + * @max 1.0 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); + +/** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @unit %/rad + * @min 0.005 + * @max 0.5 + * @decimal 3 + * @increment 0.005 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); + +/** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); + +/** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f); + +/** + * Roll rate feed forward + * + * Direct feed forward from rate setpoint to control surface output. Use this + * to obtain a tigher response of the controller without introducing + * noise amplification. + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f); + +/** + * Pitch rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); + +/** + * Yaw rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); + +/** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @unit %/rad/s + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.05 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); + +/** + * Pitch setpoint offset (pitch at level flight) + * + * An airframe specific offset of the pitch setpoint in degrees, the value is + * added to the pitch setpoint and should correspond to the pitch at + * typical cruise speed of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); + +/** + * Max manual roll + * + * Max roll for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); + +/** + * Max manual pitch + * + * Max pitch for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @increment 0.5 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); + +/** + * Scale factor for flaps + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); + +/** + * Flaps setting during take-off + * + * Sets a fraction of full flaps (FW_FLAPS_SCL) during take-off + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f); + +/** + * Flaps setting during landing + * + * Sets a fraction of full flaps (FW_FLAPS_SCL) during landing + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); + +/** + * Scale factor for flaperons + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); + +/** + * Airspeed mode + * + * For small wings or VTOL without airspeed sensor this parameter can be used to + * enable flying without an airspeed reading + * + * @value 0 Normal (use airspeed if available) + * @value 1 Airspeed disabled + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_ARSP_MODE, 0); + +/** + * Enable airspeed scaling + * + * This enables a logic that automatically adjusts the output of the rate controller to take + * into account the real torque produced by an aerodynamic control surface given + * the current deviation from the trim airspeed (FW_AIRSPD_TRIM). + * + * Enable when using aerodynamic control surfaces (e.g.: plane) + * Disable when using rotor wings (e.g.: autogyro) + * + * @boolean + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1); + +/** + * Manual roll scale + * + * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f); + +/** + * Manual pitch scale + * + * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f); + +/** + * Manual yaw scale + * + * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows + * to adjust the throws of the control surfaces. + * + * @unit norm + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f); + +/** + * Whether to scale throttle by battery power level + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The fixed wing + * should constantly behave as if it was fully charged with reduced max thrust + * at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0); + +/** + * Acro body x max rate. + * + * This is the rate the controller is trying to achieve if the user applies full roll + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit deg + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90); + +/** + * Acro body y max rate. + * + * This is the body y rate the controller is trying to achieve if the user applies full pitch + * stick input in acro mode. + * + * @min 45 + * @max 720 + * @unit deg + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90); + +/** + * Acro body z max rate. + * + * This is the body z rate the controller is trying to achieve if the user applies full yaw + * stick input in acro mode. + * + * @min 10 + * @max 180 + * @unit deg + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); + +/** +* Roll trim increment at minimum airspeed +* +* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f); + +/** +* Pitch trim increment at minimum airspeed +* +* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f); + +/** +* Yaw trim increment at minimum airspeed +* +* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f); + +/** +* Roll trim increment at maximum airspeed +* +* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MAX. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f); + +/** +* Pitch trim increment at maximum airspeed +* +* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MAX. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f); + +/** +* Yaw trim increment at maximum airspeed +* +* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MAX. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f); + +/** + * Roll trim increment for flaps configuration + * + * This increment is added to TRIM_ROLL whenever flaps are fully deployed. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f); + +/** + * Pitch trim increment for flaps configuration + * + * This increment is added to the pitch trim whenever flaps are fully deployed. + * + * @group FW Attitude Control + * @min -0.25 + * @max 0.25 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f); diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/prometheus_px4/src/modules/fw_pos_control_l1/CMakeLists.txt new file mode 100644 index 0000000..68e5b0a --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(launchdetection) +add_subdirectory(runway_takeoff) + +px4_add_module( + MODULE modules__fw_pos_control_l1 + MAIN fw_pos_control_l1 + SRCS + FixedwingPositionControl.cpp + FixedwingPositionControl.hpp + DEPENDS + l1 + launchdetection + landing_slope + runway_takeoff + tecs + ) diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/prometheus_px4/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp new file mode 100644 index 0000000..8093fd5 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -0,0 +1,2027 @@ +/**************************************************************************** + * + * 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 "FixedwingPositionControl.hpp" + +#include + +using math::constrain; +using math::max; +using math::min; +using math::radians; + +using matrix::Dcmf; +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector2d; +using matrix::Vector3f; +using matrix::wrap_pi; + +FixedwingPositionControl::FixedwingPositionControl(bool vtol) : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), + _launchDetector(this), + _runway_takeoff(this) +{ + if (vtol) { + _param_handle_airspeed_trans = param_find("VT_ARSP_TRANS"); + + // VTOL parameter VTOL_TYPE + int32_t vt_type = -1; + param_get(param_find("VT_TYPE"), &vt_type); + + _vtol_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + } + + // limit to 50 Hz + _local_pos_sub.set_interval_ms(20); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingPositionControl::~FixedwingPositionControl() +{ + perf_free(_loop_perf); +} + +bool +FixedwingPositionControl::init() +{ + if (!_local_pos_sub.registerCallback()) { + PX4_ERR("vehicle local position callback registration failed!"); + return false; + } + + return true; +} + +int +FixedwingPositionControl::parameters_update() +{ + updateParams(); + + // VTOL parameter VT_ARSP_TRANS + if (_param_handle_airspeed_trans != PARAM_INVALID) { + param_get(_param_handle_airspeed_trans, &_param_airspeed_trans); + } + + // L1 control parameters + _l1_control.set_l1_damping(_param_fw_l1_damping.get()); + _l1_control.set_l1_period(_param_fw_l1_period.get()); + _l1_control.set_l1_roll_limit(radians(_param_fw_r_lim.get())); + _l1_control.set_roll_slew_rate(radians(_param_fw_l1_r_slew_max.get())); + + // TECS parameters + _tecs.set_max_climb_rate(_param_fw_t_clmb_max.get()); + _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); + _tecs.set_speed_weight(_param_fw_t_spdweight.get()); + _tecs.set_equivalent_airspeed_cruise(_param_fw_airspd_trim.get()); + _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); + _tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get()); + _tecs.set_min_sink_rate(_param_fw_t_sink_min.get()); + _tecs.set_throttle_damp(_param_fw_t_thr_damp.get()); + _tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get()); + _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); + _tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get()); + _tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get()); + _tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get()); + _tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); + _tecs.set_pitch_damping(_param_fw_t_ptch_damp.get()); + _tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get()); + _tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get()); + _tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); + _tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get()); + _tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get()); + _tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); + + + // Landing slope + /* check if negative value for 2/3 of flare altitude is set for throttle cut */ + float land_thrust_lim_alt_relative = _param_fw_lnd_tlalt.get(); + + if (land_thrust_lim_alt_relative < 0.0f) { + land_thrust_lim_alt_relative = 0.66f * _param_fw_lnd_flalt.get(); + } + + _landingslope.update(radians(_param_fw_lnd_ang.get()), _param_fw_lnd_flalt.get(), land_thrust_lim_alt_relative, + _param_fw_lnd_hvirt.get()); + + landing_status_publish(); + + int check_ret = PX4_OK; + + // sanity check parameters + if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) { + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min"); + check_ret = PX4_ERROR; + } + + if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) { + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s"); + check_ret = PX4_ERROR; + } + + if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() || + _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) { + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds"); + check_ret = PX4_ERROR; + } + + if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get() * 0.9f) { + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min"); + check_ret = PX4_ERROR; + } + + return check_ret; +} + +void +FixedwingPositionControl::vehicle_control_mode_poll() +{ + if (_control_mode_sub.updated()) { + const bool was_armed = _control_mode.flag_armed; + + if (_control_mode_sub.copy(&_control_mode)) { + + // reset state when arming + if (!was_armed && _control_mode.flag_armed) { + reset_takeoff_state(true); + reset_landing_state(); + } + } + } +} + +void +FixedwingPositionControl::vehicle_command_poll() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { + // only abort landing before point of no return (horizontal and vertical) + if (_control_mode.flag_control_auto_enabled && + _pos_sp_triplet.current.valid && + (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) { + + abort_landing(true); + } + } + + } +} + +void +FixedwingPositionControl::airspeed_poll() +{ + bool airspeed_valid = _airspeed_valid; + airspeed_validated_s airspeed_validated; + + if ((_param_fw_arsp_mode.get() == 0) && _airspeed_validated_sub.update(&airspeed_validated)) { + + _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed + + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) + && (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) { + + airspeed_valid = true; + + _airspeed_last_valid = airspeed_validated.timestamp; + _airspeed = airspeed_validated.calibrated_airspeed_m_s; + + _eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f); + } + + } else { + // no airspeed updates for one second + if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) { + airspeed_valid = false; + } + } + + // update TECS if validity changed + if (airspeed_valid != _airspeed_valid) { + _tecs.enable_airspeed(airspeed_valid); + _airspeed_valid = airspeed_valid; + } +} + +void +FixedwingPositionControl::manual_control_setpoint_poll() +{ + _manual_control_setpoint_sub.update(&_manual_control_setpoint); + + _manual_control_setpoint_altitude = _manual_control_setpoint.x; + _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + + if (_param_fw_posctl_inv_st.get()) { + /* Alternate stick allocation (similar concept as for multirotor systems: + * demanding up/down with the throttle stick, and move faster/break with the pitch one. + */ + _manual_control_setpoint_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f); + _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f; + } +} + + +void +FixedwingPositionControl::vehicle_attitude_poll() +{ + vehicle_attitude_s att; + + if (_vehicle_attitude_sub.update(&att)) { + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&angular_velocity); + const Vector3f rates{angular_velocity.xyz}; + + Dcmf R{Quatf(att.q)}; + + // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset + // between multirotor and fixed wing flight + if (_vtol_tailsitter) { + const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}}; + R = R * R_offset; + + _yawrate = rates(0); + + } else { + _yawrate = rates(2); + } + + const Eulerf euler_angles(R); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + + _body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; + _body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz}; + + // update TECS load factor + const float load_factor = 1.f / cosf(euler_angles(0)); + _tecs.set_load_factor(load_factor); + } +} + +float +FixedwingPositionControl::get_demanded_airspeed() +{ + float altctrl_airspeed = 0; + + // neutral throttle corresponds to trim airspeed + if (_manual_control_setpoint_airspeed < 0.5f) { + // lower half of throttle is min to trim airspeed + altctrl_airspeed = _param_fw_airspd_min.get() + + (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * + _manual_control_setpoint_airspeed * 2; + + } else { + // upper half of throttle is trim to max airspeed + altctrl_airspeed = _param_fw_airspd_trim.get() + + (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) * + (_manual_control_setpoint_airspeed * 2 - 1); + } + + return altctrl_airspeed; +} + +float +FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed) +{ + /* + * Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed. + * + * We don't know the stall speed of the aircraft, but assuming user defined + * minimum airspeed (FW_AIRSPD_MIN) is slightly larger than stall speed + * this is close enough. + * + * increase lift vector to balance additional weight in bank + * cos(bank angle) = W/L = 1/n + * n is the load factor + * + * lift is proportional to airspeed^2 so the increase in stall speed is + * Vsacc = Vs * sqrt(n) + * + */ + float adjusted_min_airspeed = _param_fw_airspd_min.get(); + + if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) { + + adjusted_min_airspeed = constrain(_param_fw_airspd_min.get() / sqrtf(cosf(_att_sp.roll_body)), + _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); + } + + // groundspeed undershoot + if (!_l1_control.circle_mode()) { + /* + * This error value ensures that a plane (as long as its throttle capability is + * not exceeded) travels towards a waypoint (and is not pushed more and more away + * by wind). Not countering this would lead to a fly-away. + */ + const float ground_speed_body = _body_velocity(0); + + if (ground_speed_body < _param_fw_gnd_spd_min.get()) { + airspeed_demand += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f); + } + } + + // add minimum ground speed undershoot (only non-zero in presence of sufficient wind) + // sanity check: limit to range + return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get()); +} + +void +FixedwingPositionControl::tecs_status_publish() +{ + tecs_status_s t{}; + + switch (_tecs.tecs_mode()) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = tecs_status_s::TECS_MODE_NORMAL; + break; + + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; + break; + + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; + break; + + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; + break; + } + + t.altitude_sp = _tecs.hgt_setpoint(); + t.altitude_filtered = _tecs.vert_pos_state(); + + t.true_airspeed_sp = _tecs.TAS_setpoint_adj(); + t.true_airspeed_filtered = _tecs.tas_state(); + + t.height_rate_setpoint = _tecs.hgt_rate_setpoint(); + t.height_rate = _tecs.vert_vel_state(); + + t.equivalent_airspeed_sp = _tecs.get_EAS_setpoint(); + t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint(); + t.true_airspeed_derivative = _tecs.speed_derivative(); + t.true_airspeed_derivative_raw = _tecs.speed_derivative_raw(); + t.true_airspeed_innovation = _tecs.getTASInnovation(); + + t.total_energy_error = _tecs.STE_error(); + t.total_energy_rate_error = _tecs.STE_rate_error(); + + t.energy_distribution_error = _tecs.SEB_error(); + t.energy_distribution_rate_error = _tecs.SEB_rate_error(); + + t.total_energy = _tecs.STE(); + t.total_energy_rate = _tecs.STE_rate(); + t.total_energy_balance = _tecs.SEB(); + t.total_energy_balance_rate = _tecs.SEB_rate(); + + t.total_energy_sp = _tecs.STE_setpoint(); + t.total_energy_rate_sp = _tecs.STE_rate_setpoint(); + t.total_energy_balance_sp = _tecs.SEB_setpoint(); + t.total_energy_balance_rate_sp = _tecs.SEB_rate_setpoint(); + + t.throttle_integ = _tecs.throttle_integ_state(); + t.pitch_integ = _tecs.pitch_integ_state(); + + t.throttle_sp = _tecs.get_throttle_setpoint(); + t.pitch_sp_rad = _tecs.get_pitch_setpoint(); + + t.timestamp = hrt_absolute_time(); + + _tecs_status_pub.publish(t); +} + +void +FixedwingPositionControl::status_publish() +{ + position_controller_status_s pos_ctrl_status = {}; + + pos_ctrl_status.nav_roll = _att_sp.roll_body; + pos_ctrl_status.nav_pitch = _att_sp.pitch_body; + pos_ctrl_status.nav_bearing = _l1_control.nav_bearing(); + + pos_ctrl_status.target_bearing = _l1_control.target_bearing(); + pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); + + pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); + + pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f); + + pos_ctrl_status.yaw_acceptance = NAN; + + pos_ctrl_status.timestamp = hrt_absolute_time(); + + pos_ctrl_status.type = _type; + + _pos_ctrl_status_pub.publish(pos_ctrl_status); +} + +void +FixedwingPositionControl::landing_status_publish() +{ + position_controller_landing_status_s pos_ctrl_landing_status = {}; + + pos_ctrl_landing_status.slope_angle_rad = _landingslope.landing_slope_angle_rad(); + pos_ctrl_landing_status.horizontal_slope_displacement = _landingslope.horizontal_slope_displacement(); + pos_ctrl_landing_status.flare_length = _landingslope.flare_length(); + + pos_ctrl_landing_status.abort_landing = _land_abort; + + pos_ctrl_landing_status.timestamp = hrt_absolute_time(); + + _pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status); +} + +void +FixedwingPositionControl::abort_landing(bool abort) +{ + // only announce changes + if (abort && !_land_abort) { + mavlink_log_critical(&_mavlink_log_pub, "Landing aborted"); + } + + _land_abort = abort; + landing_status_publish(); +} + +void +FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev, + position_setpoint_s &waypoint_next, bool flag_init) +{ + position_setpoint_s temp_prev = waypoint_prev; + position_setpoint_s temp_next = waypoint_next; + + if (flag_init) { + // previous waypoint: HDG_HOLD_SET_BACK_DIST meters behind us + waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading + radians(180.0f), + HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon); + + // next waypoint: HDG_HOLD_DIST_NEXT meters in front of us + waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading, + HDG_HOLD_DIST_NEXT, &temp_next.lat, &temp_next.lon); + + } else { + // use the existing flight path from prev to next + + // previous waypoint: shifted HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST + create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon, + HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon); + + // next waypoint: shifted -(HDG_HOLD_DIST_NEXT + HDG_HOLD_REACHED_DIST) + create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon, + -(HDG_HOLD_REACHED_DIST + HDG_HOLD_DIST_NEXT), &temp_next.lat, &temp_next.lon); + } + + waypoint_prev = temp_prev; + waypoint_prev.alt = _hold_alt; + waypoint_prev.valid = true; + + waypoint_next = temp_next; + waypoint_next.alt = _hold_alt; + waypoint_next.valid = true; +} + +float +FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt) +{ + float terrain_alt = _local_pos.ref_alt - (_local_pos.dist_bottom + _local_pos.z); + + if (PX4_ISFINITE(terrain_alt) && _local_pos.dist_bottom_valid) { + return terrain_alt; + } + + return takeoff_alt; +} + +void +FixedwingPositionControl::update_desired_altitude(float dt) +{ + /* + * The complete range is -1..+1, so this is 6% + * of the up or down range or 3% of the total range. + */ + const float deadBand = 0.06f; + + /* + * The correct scaling of the complete range needs + * to account for the missing part of the slope + * due to the deadband + */ + const float factor = 1.0f - deadBand; + + /* + * Reset the hold altitude to the current altitude if the uncertainty + * changes significantly. + * This is to guard against uncommanded altitude changes + * when the altitude certainty increases or decreases. + */ + + if (fabsf(_althold_epv - _local_pos.epv) > ALTHOLD_EPV_RESET_THRESH) { + _hold_alt = _current_altitude; + _althold_epv = _local_pos.epv; + } + + /* + * Manual control has as convention the rotation around + * an axis. Positive X means to rotate positively around + * the X axis in NED frame, which is pitching down + */ + if (_manual_control_setpoint_altitude > deadBand) { + /* pitching down */ + float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor; + + if (pitch * _param_sinkrate_target.get() < _tecs.hgt_rate_setpoint()) { + _hold_alt += (_param_sinkrate_target.get() * dt) * pitch; + _manual_height_rate_setpoint_m_s = pitch * _param_sinkrate_target.get(); + _was_in_deadband = false; + } + + } else if (_manual_control_setpoint_altitude < - deadBand) { + /* pitching up */ + float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor; + + if (pitch * _param_climbrate_target.get() > _tecs.hgt_rate_setpoint()) { + _hold_alt += (_param_climbrate_target.get() * dt) * pitch; + _manual_height_rate_setpoint_m_s = pitch * _param_climbrate_target.get(); + _was_in_deadband = false; + } + + } else if (!_was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ + _hold_alt = _current_altitude; + _althold_epv = _local_pos.epv; + _was_in_deadband = true; + _manual_height_rate_setpoint_m_s = NAN; + } + + if (_vehicle_status.is_vtol) { + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) { + _hold_alt = _current_altitude; + } + } + +} + +bool +FixedwingPositionControl::in_takeoff_situation() +{ + // a VTOL does not need special takeoff handling + if (_vehicle_status.is_vtol) { + return false; + } + + // in air for < 10s + return (hrt_elapsed_time(&_time_went_in_air) < 10_s) + && (_current_altitude <= _takeoff_ground_alt + _param_fw_clmbout_diff.get()); +} + +void +FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min) +{ + /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ + if (in_takeoff_situation()) { + *hold_altitude = _takeoff_ground_alt + _param_fw_clmbout_diff.get(); + *pitch_limit_min = radians(10.0f); + } +} + +bool +FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2d &curr_pos, + const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next) +{ + const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f); + _control_position_last_called = now; + + _l1_control.set_dt(dt); + + /* only run position controller in fixed-wing mode and during transitions for VTOL */ + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) { + _control_mode_current = FW_POSCTRL_MODE_OTHER; + return false; + } + + bool setpoint = true; + + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps + + Vector2f nav_speed_2d{ground_speed}; + + if (_airspeed_valid) { + // l1 navigation logic breaks down when wind speed exceeds max airspeed + // compute 2D groundspeed from airspeed-heading projection + const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)}; + + // angle between air_speed_2d and ground_speed + const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length())); + + // if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection + if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) { + nav_speed_2d = air_speed_2d; + } + } + + /* no throttle limit as default */ + float throttle_max = 1.0f; + + /* save time when airplane is in air */ + if (!_was_in_air && !_landed) { + _was_in_air = true; + _time_went_in_air = now; + _takeoff_ground_alt = _current_altitude; + } + + /* reset flag when airplane landed */ + if (_landed) { + _was_in_air = false; + } + + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + _tecs.reset_state(); + } + + if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr.valid) { + /* AUTONOMOUS FLIGHT */ + + _control_mode_current = FW_POSCTRL_MODE_AUTO; + + /* reset hold altitude */ + _hold_alt = _current_altitude; + + /* reset hold yaw */ + _hdg_hold_yaw = _yaw; + + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); + + /* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_param_fw_t_spdweight.get()); + _tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get()); + + Vector2d curr_wp{0, 0}; + Vector2d prev_wp{0, 0}; + + if (_vehicle_status.in_transition_to_fw) { + + if (!PX4_ISFINITE(_transition_waypoint(0))) { + double lat_transition, lon_transition; + // create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track + // during the transition + waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition, + &lon_transition); + + _transition_waypoint(0) = lat_transition; + _transition_waypoint(1) = lon_transition; + } + + + curr_wp = prev_wp = _transition_waypoint; + + } else { + /* current waypoint (the one currently heading for) */ + curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); + + if (pos_sp_prev.valid) { + prev_wp(0) = pos_sp_prev.lat; + prev_wp(1) = pos_sp_prev.lon; + + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp(0) = pos_sp_curr.lat; + prev_wp(1) = pos_sp_curr.lon; + } + + + /* reset transition waypoint, will be set upon entering front transition */ + _transition_waypoint(0) = static_cast(NAN); + _transition_waypoint(1) = static_cast(NAN); + } + + /* Initialize attitude controller integrator reset flags to 0 */ + _att_sp.roll_reset_integral = false; + _att_sp.pitch_reset_integral = false; + _att_sp.yaw_reset_integral = false; + + float mission_airspeed = _param_fw_airspd_trim.get(); + + if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && + pos_sp_curr.cruising_speed > 0.1f) { + + mission_airspeed = pos_sp_curr.cruising_speed; + } + + float mission_throttle = _param_fw_thr_cruise.get(); + + if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) && + pos_sp_curr.cruising_throttle >= 0.0f) { + mission_throttle = pos_sp_curr.cruising_throttle; + } + + float tecs_fw_thr_min; + float tecs_fw_thr_max; + float tecs_fw_mission_throttle; + + if (mission_throttle < _param_fw_thr_min.get()) { + /* enable gliding with this waypoint */ + _tecs.set_speed_weight(2.0f); + tecs_fw_thr_min = 0.0; + tecs_fw_thr_max = 0.0; + tecs_fw_mission_throttle = 0.0; + + } else { + tecs_fw_thr_min = _param_fw_thr_min.get(); + tecs_fw_thr_max = _param_fw_thr_max.get(); + tecs_fw_mission_throttle = mission_throttle; + } + + const float acc_rad = _l1_control.switch_distance(500.0f); + + uint8_t position_sp_type = pos_sp_curr.type; + + if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + // TAKEOFF: handle like a regular POSITION setpoint if already flying + if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) { + // SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION + position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } + + } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION + || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + + float dist_xy = -1.f; + float dist_z = -1.f; + + const float dist = get_distance_to_point_global_wgs84( + (double)curr_wp(0), (double)curr_wp(1), pos_sp_curr.alt, + _current_latitude, _current_longitude, _current_altitude, + &dist_xy, &dist_z); + + if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + // POSITION: achieve position setpoint altitude via loiter + // close to waypoint, but altitude error greater than twice acceptance + if ((dist >= 0.f) + && (dist_z > 2.f * _param_fw_clmbout_diff.get()) + && (dist_xy < 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) { + // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER + position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; + } + + } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + // LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER + if ((dist >= 0.f) + && (dist_z > 2.f * _param_fw_clmbout_diff.get()) + && (dist_xy > 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) { + // SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION + position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } + } + } + + _type = position_sp_type; + + + if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust_body[0] = 0.0f; + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = radians(_param_fw_psp_off.get()); + + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + // waypoint is a plain navigation waypoint + float position_sp_alt = pos_sp_curr.alt; + + // Altitude first order hold (FOH) + if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) && + ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) || + (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) || + (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) + ) { + const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1), + pos_sp_prev.lat, pos_sp_prev.lon); + + // Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one + if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) { + // Calculate distance to current waypoint + const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1), + _current_latitude, _current_longitude); + + // Save distance to waypoint if it is the smallest ever achieved, however make sure that + // _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp + _min_current_sp_distance_xy = math::min(math::min(d_curr, _min_current_sp_distance_xy), d_curr_prev); + + // if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt + // navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude + if (_min_current_sp_distance_xy > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) { + // The setpoint is set linearly and such that the system reaches the current altitude at the acceptance + // radius around the current waypoint + const float delta_alt = (pos_sp_curr.alt - pos_sp_prev.alt); + const float grad = -delta_alt / (d_curr_prev - math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))); + const float a = pos_sp_prev.alt - grad * d_curr_prev; + + position_sp_alt = a + grad * _min_current_sp_distance_xy; + } + } + } + + _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d); + _att_sp.roll_body = _l1_control.get_roll_setpoint(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + tecs_update_pitch_throttle(now, position_sp_alt, + calculate_target_airspeed(mission_airspeed, ground_speed), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + tecs_fw_thr_min, + tecs_fw_thr_max, + tecs_fw_mission_throttle, + false, + radians(_param_fw_p_lim_min.get())); + + + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + /* waypoint is a loiter waypoint */ + float loiter_radius = pos_sp_curr.loiter_radius; + uint8_t loiter_direction = pos_sp_curr.loiter_direction; + + if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) { + loiter_radius = _param_nav_loiter_rad.get(); + loiter_direction = (loiter_radius > 0) ? 1 : -1; + + } + + _l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, nav_speed_2d); + + _att_sp.roll_body = _l1_control.get_roll_setpoint(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + float alt_sp = pos_sp_curr.alt; + + if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid + && _l1_control.circle_mode() && _param_fw_lnd_earlycfg.get()) { + // We're in a loiter directly before a landing WP. Enable our landing configuration (flaps, + // landing airspeed and potentially tighter altitude control) already such that we don't + // have to do this switch (which can cause significant altitude errors) close to the ground. + _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); + mission_airspeed = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); + _att_sp.apply_flaps = true; + } + + if (in_takeoff_situation()) { + alt_sp = max(alt_sp, _takeoff_ground_alt + _param_fw_clmbout_diff.get()); + _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f)); + } + + if (_land_abort) { + if (pos_sp_curr.alt - _current_altitude < _param_fw_clmbout_diff.get()) { + // aborted landing complete, normal loiter over landing point + abort_landing(false); + + } else { + // continue straight until vehicle has sufficient altitude + _att_sp.roll_body = 0.0f; + } + + _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); + } + + tecs_update_pitch_throttle(now, alt_sp, + calculate_target_airspeed(mission_airspeed, ground_speed), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + tecs_fw_thr_min, + tecs_fw_thr_max, + tecs_fw_mission_throttle, + false, + radians(_param_fw_p_lim_min.get())); + + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND) { + control_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + control_takeoff(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + } + + /* reset landing state */ + if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) { + reset_landing_state(); + } + + /* reset takeoff/launch state */ + if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + reset_takeoff_state(); + } + + if (was_circle_mode && !_l1_control.circle_mode()) { + /* just kicked out of loiter, reset roll integrals */ + _att_sp.roll_reset_integral = true; + } + + } else if (_control_mode.flag_control_velocity_enabled && + _control_mode.flag_control_altitude_enabled) { + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, + heading is set to a distant waypoint */ + + if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + /* Need to init because last loop iteration was in a different mode */ + _hold_alt = _current_altitude; + _hdg_hold_yaw = _yaw; + _hdg_hold_enabled = false; // this makes sure the waypoints are reset below + _yaw_lock_engaged = false; + + /* reset setpoints from other modes (auto) otherwise we won't + * level out without new manual input */ + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + _att_sp.yaw_body = 0; + } + + _control_mode_current = FW_POSCTRL_MODE_POSITION; + + float altctrl_airspeed = get_demanded_airspeed(); + + /* update desired altitude based on user pitch stick input */ + update_desired_altitude(dt); + + // if we assume that user is taking off then help by demanding altitude setpoint well above ground + // and set limit to pitch angle to prevent steering into ground + // this will only affect planes and not VTOL + float pitch_limit_min = _param_fw_p_lim_min.get(); + do_takeoff_help(&_hold_alt, &pitch_limit_min); + + /* throttle limiting */ + throttle_max = _param_fw_thr_max.get(); + + if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) { + throttle_max = 0.0f; + } + + tecs_update_pitch_throttle(now, _hold_alt, + altctrl_airspeed, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + throttle_max, + _param_fw_thr_cruise.get(), + false, + pitch_limit_min, + tecs_status_s::TECS_MODE_NORMAL, + _manual_height_rate_setpoint_m_s); + + /* heading control */ + if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH && + fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) { + + /* heading / roll is zero, lock onto current heading */ + if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + // little yaw movement, lock to current heading + _yaw_lock_engaged = true; + + } + + /* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration + to make sure the plane does not start rolling + */ + if (in_takeoff_situation()) { + _hdg_hold_enabled = false; + _yaw_lock_engaged = true; + } + + if (_yaw_lock_engaged) { + + /* just switched back from non heading-hold to heading hold */ + if (!_hdg_hold_enabled) { + _hdg_hold_enabled = true; + _hdg_hold_yaw = _yaw; + + get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + } + + /* we have a valid heading hold position, are we too close? */ + float dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _hdg_hold_curr_wp.lat, + _hdg_hold_curr_wp.lon); + + if (dist < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + } + + Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon}; + Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon}; + + /* populate l1 control setpoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); + + _att_sp.roll_body = _l1_control.get_roll_setpoint(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + if (in_takeoff_situation()) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f)); + } + } + } + + if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH || + fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) { + + _hdg_hold_enabled = false; + _yaw_lock_engaged = false; + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + _att_sp.yaw_body = 0; + } + + } else if (_control_mode.flag_control_altitude_enabled) { + /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + + if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) { + /* Need to init because last loop iteration was in a different mode */ + _hold_alt = _current_altitude; + } + + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; + + /* Get demanded airspeed */ + float altctrl_airspeed = get_demanded_airspeed(); + + /* update desired altitude based on user pitch stick input */ + update_desired_altitude(dt); + + // if we assume that user is taking off then help by demanding altitude setpoint well above ground + // and set limit to pitch angle to prevent steering into ground + // this will only affect planes and not VTOL + float pitch_limit_min = _param_fw_p_lim_min.get(); + do_takeoff_help(&_hold_alt, &pitch_limit_min); + + /* throttle limiting */ + throttle_max = _param_fw_thr_max.get(); + + if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) { + throttle_max = 0.0f; + } + + tecs_update_pitch_throttle(now, _hold_alt, + altctrl_airspeed, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + throttle_max, + _param_fw_thr_cruise.get(), + false, + pitch_limit_min, + tecs_status_s::TECS_MODE_NORMAL, + _manual_height_rate_setpoint_m_s); + + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + _att_sp.yaw_body = 0; + + } else { + _control_mode_current = FW_POSCTRL_MODE_OTHER; + + /* do not publish the setpoint */ + setpoint = false; + + // reset hold altitude + _hold_alt = _current_altitude; + + /* reset landing and takeoff state */ + if (!_last_manual) { + reset_landing_state(); + reset_takeoff_state(); + } + } + + /* Copy thrust output for publication */ + if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto + pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_runway_takeoff.runwayTakeoffEnabled()) { + + /* making sure again that the correct thrust is used, + * without depending on library calls for safety reasons. + the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ + _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _runway_takeoff.runwayTakeoffEnabled()) { + + _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, min(get_tecs_thrust(), throttle_max)); + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + + _att_sp.thrust_body[0] = 0.0f; + + } else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); + + } else { + /* Copy thrust and pitch values from tecs */ + if (_landed) { + // when we are landed state we want the motor to spin at idle speed + _att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max); + + } else { + _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); + } + } + + // decide when to use pitch setpoint from TECS because in some cases pitch + // setpoint is generated by other means + bool use_tecs_pitch = true; + + // auto runway takeoff + use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())); + + // flaring during landing + use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical); + + // manual attitude control + use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER); + + if (use_tecs_pitch) { + _att_sp.pitch_body = get_tecs_pitch(); + } + + if (_control_mode.flag_control_position_enabled) { + _last_manual = false; + + } else { + _last_manual = true; + } + + return setpoint; +} + +void +FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +{ + /* current waypoint (the one currently heading for) */ + Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon); + Vector2d prev_wp{0, 0}; /* previous waypoint */ + + if (pos_sp_prev.valid) { + prev_wp(0) = pos_sp_prev.lat; + prev_wp(1) = pos_sp_prev.lon; + + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp(0) = pos_sp_curr.lat; + prev_wp(1) = pos_sp_curr.lon; + } + + // apply flaps for takeoff according to the corresponding scale factor set + // via FW_FLAPS_TO_SCL + _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF; + + // continuously reset launch detection and runway takeoff until armed + if (!_control_mode.flag_armed) { + _launchDetector.reset(); + _launch_detection_state = LAUNCHDETECTION_RES_NONE; + _launch_detection_notify = 0; + } + + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + _runway_takeoff.init(now, _yaw, _current_latitude, _current_longitude); + + /* need this already before takeoff is detected + * doesn't matter if it gets reset when takeoff is detected eventually */ + _takeoff_ground_alt = _current_altitude; + + mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway"); + } + + float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt); + + // update runway takeoff helper + _runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt, + _current_latitude, _current_longitude, &_mavlink_log_pub); + + /* + * Update navigation: _runway_takeoff returns the start WP according to mode and phase. + * If we use the navigator heading or not is decided later. + */ + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed); + + // update tecs + const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get()); + + tecs_update_pitch_throttle(now, pos_sp_curr.alt, + calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed), + radians(_param_fw_p_lim_min.get()), + radians(takeoff_pitch_max_deg), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here? + _param_fw_thr_cruise.get(), + _runway_takeoff.climbout(), + radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())), + tecs_status_s::TECS_MODE_TAKEOFF); + + // assign values + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint()); + _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); + _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); + + // reset integrals except yaw (which also counts for the wheel controller) + _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + + } else { + /* Perform launch detection */ + if (_launchDetector.launchDetectionEnabled() && + _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + + if (_control_mode.flag_armed) { + /* Perform launch detection */ + + /* Inform user that launchdetection is running every 4s */ + if ((now - _launch_detection_notify) > 4_s) { + mavlink_log_critical(&_mavlink_log_pub, "Launch detection running"); + _launch_detection_notify = now; + } + + /* Detect launch using body X (forward) acceleration */ + _launchDetector.update(now, _body_acceleration(0)); + + /* update our copy of the launch detection state */ + _launch_detection_state = _launchDetector.getLaunchDetected(); + } + + } else { + /* no takeoff detection --> fly */ + _launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + /* Set control values depending on the detection state */ + if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. */ + + _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); + _att_sp.roll_body = _l1_control.get_roll_setpoint(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use idle throttle */ + float takeoff_throttle = _param_fw_thr_max.get(); + + if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + takeoff_throttle = _param_fw_thr_idle.get(); + } + + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get()); + const float altitude_error = pos_sp_curr.alt - _current_altitude; + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */ + if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) { + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(now, pos_sp_curr.alt, + _param_fw_airspd_trim.get(), + radians(_param_fw_p_lim_min.get()), + radians(takeoff_pitch_max_deg), + _param_fw_thr_min.get(), + takeoff_throttle, + _param_fw_thr_cruise.get(), + true, + radians(_takeoff_pitch_min.get()), + tecs_status_s::TECS_MODE_TAKEOFF); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f)); + + } else { + tecs_update_pitch_throttle(now, pos_sp_curr.alt, + calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + takeoff_throttle, + _param_fw_thr_cruise.get(), + false, + radians(_param_fw_p_lim_min.get())); + } + + } else { + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = radians(_takeoff_pitch_min.get()); + } + } +} + +void +FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +{ + /* current waypoint (the one currently heading for) */ + Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon); + Vector2d prev_wp{0, 0}; /* previous waypoint */ + + if (pos_sp_prev.valid) { + prev_wp(0) = pos_sp_prev.lat; + prev_wp(1) = pos_sp_prev.lon; + + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp(0) = pos_sp_curr.lat; + prev_wp(1) = pos_sp_curr.lon; + } + + // apply full flaps for landings. this flag will also trigger the use of flaperons + // if they have been enabled using the corresponding parameter + _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; + + // Enable tighter altitude control for landings + _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); + + // save time at which we started landing and reset abort_landing + if (_time_started_landing == 0) { + reset_landing_state(); + _time_started_landing = now; + } + + const float bearing_airplane_currwp = get_bearing_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), + (double)curr_wp(0), (double)curr_wp(1)); + + float bearing_lastwp_currwp = bearing_airplane_currwp; + + if (pos_sp_prev.valid) { + bearing_lastwp_currwp = get_bearing_to_next_waypoint((double)prev_wp(0), (double)prev_wp(1), (double)curr_wp(0), + (double)curr_wp(1)); + } + + /* Horizontal landing control */ + /* switch to heading hold for the last meters, continue heading hold after */ + float wp_distance = get_distance_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), (double)curr_wp(0), + (double)curr_wp(1)); + + /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ + float wp_distance_save = wp_distance; + + if (fabsf(wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) >= radians(90.0f)) { + wp_distance_save = 0.0f; + } + + // create virtual waypoint which is on the desired flight path but + // some distance behind landing waypoint. This will make sure that the plane + // will always follow the desired flight path even if we get close or past + // the landing waypoint + if (pos_sp_prev.valid) { + double lat = pos_sp_curr.lat; + double lon = pos_sp_curr.lon; + + create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon, + pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon); + + curr_wp(0) = lat; + curr_wp(1) = lon; + } + + // we want the plane to keep tracking the desired flight path until we start flaring + // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds + if ((_param_fw_lnd_hhdist.get() > 0.0f) && !_land_noreturn_horizontal && + ((wp_distance < _param_fw_lnd_hhdist.get()) || _land_noreturn_vertical)) { + + if (pos_sp_prev.valid) { + /* heading hold, along the line connecting this and the last waypoint */ + _target_bearing = bearing_lastwp_currwp; + + } else { + _target_bearing = _yaw; + } + + _land_noreturn_horizontal = true; + mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold"); + } + + if (_land_noreturn_horizontal) { + // heading hold + _l1_control.navigate_heading(_target_bearing, _yaw, ground_speed); + + } else { + // normal navigation + _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); + } + + _att_sp.roll_body = _l1_control.get_roll_setpoint(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + if (_land_noreturn_horizontal) { + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f)); + } + + /* Vertical landing control */ + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + + // default to no terrain estimation, just use landing waypoint altitude + float terrain_alt = pos_sp_curr.alt; + + if (_param_fw_lnd_useter.get() == 1) { + if (_local_pos.dist_bottom_valid) { + // all good, have valid terrain altitude + float terrain_vpos = _local_pos.dist_bottom + _local_pos.z; + terrain_alt = (_local_pos.ref_alt - terrain_vpos); + _t_alt_prev_valid = terrain_alt; + _time_last_t_alt = now; + + } else if (_time_last_t_alt == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if ((now - _time_started_landing) < 10_s) { + terrain_alt = pos_sp_curr.alt; + + } else { + // still no valid terrain, abort landing + terrain_alt = pos_sp_curr.alt; + abort_landing(true); + } + + } else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_ALT_TIMEOUT) + || _land_noreturn_vertical) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _t_alt_prev_valid; + + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _t_alt_prev_valid; + abort_landing(true); + } + } + + /* Check if we should start flaring with a vertical and a + * horizontal limit (with some tolerance) + * The horizontal limit is only applied when we are in front of the wp + */ + if ((_current_altitude < terrain_alt + _landingslope.flare_relative_alt()) || + _land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + + /* land with minimal speed */ + + /* force TECS to only control speed with pitch, altitude is only implicitly controlled now */ + // _tecs.set_speed_weight(2.0f); + + /* kill the throttle if param requests it */ + float throttle_max = _param_fw_thr_max.get(); + + /* enable direct yaw control using rudder/wheel */ + if (_land_noreturn_horizontal) { + _att_sp.yaw_body = _target_bearing; + _att_sp.fw_control_yaw = true; + } + + if (((_current_altitude < terrain_alt + _landingslope.motor_lim_relative_alt()) && + (wp_distance_save < _landingslope.flare_length() + 5.0f)) || // Only kill throttle when close to WP + _land_motor_lim) { + throttle_max = min(throttle_max, _param_fw_thr_lnd_max.get()); + + if (!_land_motor_lim) { + _land_motor_lim = true; + mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle"); + } + } + + float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, + bearing_airplane_currwp); + + /* avoid climbout */ + if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) { + flare_curve_alt_rel = 0.0f; // stay on ground + _land_stayonground = true; + } + + const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); + const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f; + + tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel, + calculate_target_airspeed(airspeed_land, ground_speed), + radians(_param_fw_lnd_fl_pmin.get()), + radians(_param_fw_lnd_fl_pmax.get()), + 0.0f, + throttle_max, + throttle_land, + false, + _land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()), + _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); + + if (!_land_noreturn_vertical) { + // just started with the flaring phase + _flare_pitch_sp = radians(_param_fw_psp_off.get()); + _flare_height = _current_altitude - terrain_alt; + mavlink_log_info(&_mavlink_log_pub, "Landing, flaring"); + _land_noreturn_vertical = true; + + } else { + if (_local_pos.vz > 0.1f) { + _flare_pitch_sp = radians(_param_fw_lnd_fl_pmin.get()) * + constrain((_flare_height - (_current_altitude - terrain_alt)) / _flare_height, 0.0f, 1.0f); + } + + // otherwise continue using previous _flare_pitch_sp + } + + _att_sp.pitch_body = _flare_pitch_sp; + _flare_curve_alt_rel_last = flare_curve_alt_rel; + + } else { + + /* intersect glide slope: + * minimize speed to approach speed + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below the slope continue at previous wp altitude + * until the intersection with slope + * */ + + float altitude_desired = terrain_alt; + + const float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, + bearing_lastwp_currwp, bearing_airplane_currwp); + + if (_current_altitude > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) { + /* stay on slope */ + altitude_desired = terrain_alt + landing_slope_alt_rel_desired; + + if (!_land_onslope) { + mavlink_log_info(&_mavlink_log_pub, "Landing, on slope"); + _land_onslope = true; + } + + } else { + /* continue horizontally */ + if (pos_sp_prev.valid) { + altitude_desired = pos_sp_prev.alt; + + } else { + altitude_desired = _current_altitude; + } + } + + const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); + + tecs_update_pitch_throttle(now, altitude_desired, + calculate_target_airspeed(airspeed_approach, ground_speed), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + _param_fw_thr_cruise.get(), + false, + radians(_param_fw_p_lim_min.get())); + } +} + +float +FixedwingPositionControl::get_tecs_pitch() +{ + if (_is_tecs_running) { + return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get()); + } + + // return level flight pitch offset to prevent stale tecs state when it's not running + return radians(_param_fw_psp_off.get()); +} + +float +FixedwingPositionControl::get_tecs_thrust() +{ + if (_is_tecs_running) { + return _tecs.get_throttle_setpoint(); + } + + // return 0 to prevent stale tecs state when it's not running + return 0.0f; +} + +void +FixedwingPositionControl::Run() +{ + if (should_exit()) { + _local_pos_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + /* only run controller if position changed */ + + if (_local_pos_sub.update(&_local_pos)) { + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + parameters_update(); + } + + vehicle_global_position_s gpos; + + if (_global_pos_sub.update(&gpos)) { + _current_latitude = gpos.lat; + _current_longitude = gpos.lon; + } + + _current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters + + // handle estimator reset events. we only adjust setpoins for manual modes + if (_control_mode.flag_control_manual_enabled) { + if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) { + _hold_alt += -_local_pos.delta_z; + // make TECS accept step in altitude and demanded altitude + _tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude); + } + + // adjust navigation waypoints in position control mode + if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled + && _local_pos.vxy_reset_counter != _pos_reset_counter) { + + // reset heading hold flag, which will re-initialise position control + _hdg_hold_enabled = false; + } + } + + // update the reset counters in any case + _alt_reset_counter = _local_pos.vz_reset_counter; + _pos_reset_counter = _local_pos.vxy_reset_counter; + + + if (_control_mode.flag_control_offboard_enabled) { + // Convert Local setpoints to global setpoints + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) { + + map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_timestamp); + _global_local_alt0 = _local_pos.ref_alt; + } + + vehicle_local_position_setpoint_s trajectory_setpoint; + + if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { + if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) { + double lat; + double lon; + + if (map_projection_reproject(&_global_local_proj_ref, trajectory_setpoint.x, trajectory_setpoint.y, &lat, &lon) == 0) { + _pos_sp_triplet = {}; // clear any existing + + _pos_sp_triplet.timestamp = trajectory_setpoint.timestamp; + _pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _pos_sp_triplet.current.lat = lat; + _pos_sp_triplet.current.lon = lon; + _pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z; + _pos_sp_triplet.current.cruising_speed = NAN; // ignored + _pos_sp_triplet.current.cruising_throttle = NAN; // ignored + } + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint"); + } + } + + } else { + if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) { + // reset the altitude foh (first order hold) logic + _min_current_sp_distance_xy = FLT_MAX; + } + } + + airspeed_poll(); + manual_control_setpoint_poll(); + vehicle_attitude_poll(); + vehicle_command_poll(); + vehicle_control_mode_poll(); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + } + } + + _vehicle_status_sub.update(&_vehicle_status); + + Vector2d curr_pos(_current_latitude, _current_longitude); + Vector2f ground_speed(_local_pos.vx, _local_pos.vy); + + /* + * Attempt to control position, on success (= sensors present and not in manual mode), + * publish setpoint. + */ + if (control_position(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, + _pos_sp_triplet.next)) { + + if (_control_mode.flag_control_manual_enabled) { + _att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_man_r_max.get()), + radians(_param_fw_man_r_max.get())); + _att_sp.pitch_body = constrain(_att_sp.pitch_body, -radians(_param_fw_man_p_max.get()), + radians(_param_fw_man_p_max.get())); + } + + if (_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_acceleration_enabled || + _control_mode.flag_control_altitude_enabled) { + + const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + q.copyTo(_att_sp.q_d); + + _att_sp.timestamp = hrt_absolute_time(); + _attitude_sp_pub.publish(_att_sp); + + // only publish status in full FW mode + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + || _vehicle_status.in_transition_mode) { + status_publish(); + + } + } + } + + perf_end(_loop_perf); + } +} + +void +FixedwingPositionControl::reset_takeoff_state(bool force) +{ + // only reset takeoff if !armed or just landed + if (!_control_mode.flag_armed || (_was_in_air && _landed) || force) { + + _runway_takeoff.reset(); + + _launchDetector.reset(); + _launch_detection_state = LAUNCHDETECTION_RES_NONE; + _launch_detection_notify = 0; + + } else { + _launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } +} + +void +FixedwingPositionControl::reset_landing_state() +{ + _time_started_landing = 0; + + // reset terrain estimation relevant values + _time_last_t_alt = 0; + + _land_noreturn_horizontal = false; + _land_noreturn_vertical = false; + _land_stayonground = false; + _land_motor_lim = false; + _land_onslope = false; + + // reset abort land, unless loitering after an abort + if (_land_abort && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) { + + abort_landing(false); + } +} + +void +FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + uint8_t mode, float hgt_rate_sp) +{ + const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f); + _last_tecs_update = now; + + // do not run TECS if we are not in air + bool run_tecs = !_landed; + + // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition + // (it should also not run during VTOL blending because airspeed is too low still) + if (_vehicle_status.is_vtol) { + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) { + run_tecs = false; + } + + if (_vehicle_status.in_transition_mode) { + // we're in transition + _was_in_transition = true; + + // set this to transition airspeed to init tecs correctly + if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) { + // some vtols fly without airspeed sensor + _asp_after_transition = _param_airspeed_trans; + + } else { + _asp_after_transition = _airspeed; + } + + _asp_after_transition = constrain(_asp_after_transition, _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); + + } else if (_was_in_transition) { + // after transition we ramp up desired airspeed from the speed we had coming out of the transition + _asp_after_transition += dt * 2.0f; // increase 2m/s + + if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) { + airspeed_sp = max(_asp_after_transition, _airspeed); + + } else { + _was_in_transition = false; + _asp_after_transition = 0.0f; + } + } + } + + _is_tecs_running = run_tecs; + + if (!run_tecs) { + // next time we run TECS we should reinitialize states + _reinitialize_tecs = true; + return; + } + + if (_reinitialize_tecs) { + _tecs.reset_state(); + _reinitialize_tecs = false; + } + + if (_vehicle_status.engine_failure) { + /* Force the slow downwards spiral */ + pitch_min_rad = radians(-1.0f); + pitch_max_rad = radians(5.0f); + } + + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND + || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); + + /* tell TECS to update its state, but let it know when it cannot actually control the plane */ + bool in_air_alt_control = (!_landed && + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled)); + + /* update TECS vehicle state estimates */ + _tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control, + _current_altitude, _local_pos.vz); + + /* scale throttle cruise by baro pressure */ + if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) { + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.copy(&air_data)) { + if (PX4_ISFINITE(air_data.baro_pressure_pa) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) { + // scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature) + const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_PA / air_data.baro_pressure_pa); + const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.f, 1.f, 2.f); + + throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f); + throttle_cruise = constrain(throttle_cruise * scale, throttle_min + 0.01f, throttle_max - 0.01f); + } + } + } + + _tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()), + _current_altitude, alt_sp, + airspeed_sp, _airspeed, _eas2tas, + climbout_mode, + climbout_pitch_min_rad - radians(_param_fw_psp_off.get()), + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad - radians(_param_fw_psp_off.get()), + pitch_max_rad - radians(_param_fw_psp_off.get()), + _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp); + + tecs_status_publish(); +} + +int FixedwingPositionControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + FixedwingPositionControl *instance = new FixedwingPositionControl(vtol); + + 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 FixedwingPositionControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FixedwingPositionControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_pos_control_l1 is the fixed wing position controller. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fw_pos_control_l1", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]) +{ + return FixedwingPositionControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/prometheus_px4/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp new file mode 100644 index 0000000..b2f84e2 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -0,0 +1,428 @@ +/**************************************************************************** + * + * 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 fw_pos_control_l1_main.hpp + * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll + * angle, equivalent to a lateral motion (for copters and rovers). + * + * The implementation for the controllers is in the ECL library. This class only + * interfaces to the library. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + */ + +#ifndef FIXEDWINGPOSITIONCONTROL_HPP_ +#define FIXEDWINGPOSITIONCONTROL_HPP_ + +#include "launchdetection/LaunchDetector.h" +#include "runway_takeoff/RunwayTakeoff.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace launchdetection; +using namespace runwaytakeoff; +using namespace time_literals; + +using matrix::Vector2d; +using matrix::Vector2f; + +static constexpr float HDG_HOLD_DIST_NEXT = + 3000.0f; // initial distance of waypoint in front of plane in heading hold mode +static constexpr float HDG_HOLD_REACHED_DIST = + 1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode +static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane +static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; // max yawrate at which plane locks yaw for heading hold mode +static constexpr float HDG_HOLD_MAN_INPUT_THRESH = + 0.01f; // max manual roll/yaw input from user which does not change the locked heading + +static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid + +static constexpr float THROTTLE_THRESH = + 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; + +class FixedwingPositionControl final : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + FixedwingPositionControl(bool vtol = false); + ~FixedwingPositionControl() 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; + + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::Publication _attitude_sp_pub; + uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication + uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication + uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication + + manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data + position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items + vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint + vehicle_control_mode_s _control_mode {}; ///< control mode + vehicle_local_position_s _local_pos {}; ///< vehicle local position + vehicle_status_s _vehicle_status {}; ///< vehicle status + + double _current_latitude{0}; + double _current_longitude{0}; + float _current_altitude{0.f}; + + perf_counter_t _loop_perf; ///< loop performance counter + + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + + float _hold_alt{0.0f}; ///< hold altitude for altitude mode + float _manual_height_rate_setpoint_m_s{NAN}; + float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched + float _hdg_hold_yaw{0.0f}; ///< hold heading for velocity mode + bool _hdg_hold_enabled{false}; ///< heading hold enabled + bool _yaw_lock_engaged{false}; ///< yaw is locked for heading hold + float _althold_epv{0.0f}; ///< the position estimate accuracy when engaging alt hold + bool _was_in_deadband{false}; ///< wether the last stick input was in althold deadband + + float _min_current_sp_distance_xy{FLT_MAX}; + + position_setpoint_s _hdg_hold_prev_wp {}; ///< position where heading hold started + position_setpoint_s _hdg_hold_curr_wp {}; ///< position to which heading hold flies + + hrt_abstime _control_position_last_called{0}; ///< last call of control_position + + bool _landed{true}; + + /* Landing */ + bool _land_noreturn_horizontal{false}; + bool _land_noreturn_vertical{false}; + bool _land_stayonground{false}; + bool _land_motor_lim{false}; + bool _land_onslope{false}; + bool _land_abort{false}; + + Landingslope _landingslope; + + hrt_abstime _time_started_landing{0}; ///< time at which landing started + + float _t_alt_prev_valid{0}; ///< last terrain estimate which was valid + hrt_abstime _time_last_t_alt{0}; ///< time at which we had last valid terrain alt + + float _flare_height{0.0f}; ///< estimated height to ground at which flare started + float _flare_pitch_sp{0.0f}; ///< Current forced (i.e. not determined using TECS) flare pitch setpoint + float _flare_curve_alt_rel_last{0.0f}; + float _target_bearing{0.0f}; ///< estimated height to ground at which flare started + + bool _was_in_air{false}; ///< indicated wether the plane was in the air in the previous interation*/ + hrt_abstime _time_went_in_air{0}; ///< time at which the plane went in the air + + /* Takeoff launch detection and runway */ + LaunchDetector _launchDetector; + LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE}; + hrt_abstime _launch_detection_notify{0}; + + RunwayTakeoff _runway_takeoff; + + bool _last_manual{false}; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) + + /* throttle and airspeed states */ + bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists + hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts. + float _airspeed{0.0f}; + float _eas2tas{1.0f}; + + float _pitch{0.0f}; + float _yaw{0.0f}; + float _yawrate{0.0f}; + + matrix::Vector3f _body_acceleration{}; + matrix::Vector3f _body_velocity{}; + + bool _reinitialize_tecs{true}; ///< indicates if the TECS states should be reinitialized (used for VTOL) + bool _is_tecs_running{false}; + hrt_abstime _last_tecs_update{0}; + + float _asp_after_transition{0.0f}; + bool _was_in_transition{false}; + + bool _vtol_tailsitter{false}; + + matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; + + // estimator reset counters + uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position + uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state + + float _manual_control_setpoint_altitude{0.0f}; + float _manual_control_setpoint_airspeed{0.0f}; + + ECL_L1_Pos_Controller _l1_control; + TECS _tecs; + + uint8_t _type{0}; + enum FW_POSCTRL_MODE { + FW_POSCTRL_MODE_AUTO, + FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_ALTITUDE, + FW_POSCTRL_MODE_OTHER + } _control_mode_current{FW_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. + + param_t _param_handle_airspeed_trans{PARAM_INVALID}; + float _param_airspeed_trans{NAN}; + + // Update our local parameter cache. + int parameters_update(); + + // Update subscriptions + void airspeed_poll(); + void control_update(); + void manual_control_setpoint_poll(); + void vehicle_attitude_poll(); + void vehicle_command_poll(); + void vehicle_control_mode_poll(); + void vehicle_status_poll(); + + void status_publish(); + void landing_status_publish(); + void tecs_status_publish(); + + void abort_landing(bool abort); + + /** + * Get a new waypoint based on heading and distance from current position + * + * @param heading the heading to fly to + * @param distance the distance of the generated waypoint + * @param waypoint_prev the waypoint at the current position + * @param waypoint_next the waypoint in the heading direction + */ + void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev, + position_setpoint_s &waypoint_next, bool flag_init); + + /** + * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available + */ + float get_terrain_altitude_takeoff(float takeoff_alt); + + /** + * Check if we are in a takeoff situation + */ + bool in_takeoff_situation(); + + /** + * Do takeoff help when in altitude controlled modes + * @param hold_altitude altitude setpoint for controller + * @param pitch_limit_min minimum pitch allowed + */ + void do_takeoff_help(float *hold_altitude, float *pitch_limit_min); + + /** + * Update desired altitude base on user pitch stick input + * + * @param dt Time step + */ + void update_desired_altitude(float dt); + + bool control_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, + const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next); + void control_takeoff(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, + const position_setpoint_s &pos_sp_curr); + void control_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, + const position_setpoint_s &pos_sp_curr); + + float get_tecs_pitch(); + float get_tecs_thrust(); + + float get_demanded_airspeed(); + float calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed); + + void reset_takeoff_state(bool force = false); + void reset_landing_state(); + + /* + * Call TECS : a wrapper function to call the TECS implementation + */ + void tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + uint8_t mode = tecs_status_s::TECS_MODE_NORMAL, float hgt_rate_sp = NAN); + + DEFINE_PARAMETERS( + + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_trim, + (ParamFloat) _param_fw_airspd_stall, + + (ParamFloat) _param_fw_clmbout_diff, + + (ParamFloat) _param_fw_gnd_spd_min, + + (ParamFloat) _param_fw_l1_damping, + (ParamFloat) _param_fw_l1_period, + (ParamFloat) _param_fw_l1_r_slew_max, + (ParamFloat) _param_fw_r_lim, + + (ParamFloat) _param_fw_lnd_airspd_sc, + (ParamFloat) _param_fw_lnd_ang, + (ParamFloat) _param_fw_lnd_fl_pmax, + (ParamFloat) _param_fw_lnd_fl_pmin, + (ParamFloat) _param_fw_lnd_flalt, + (ParamFloat) _param_fw_lnd_hhdist, + (ParamFloat) _param_fw_lnd_hvirt, + (ParamFloat) _param_fw_thrtc_sc, + (ParamFloat) _param_fw_lnd_tlalt, + (ParamBool) _param_fw_lnd_earlycfg, + (ParamBool) _param_fw_lnd_useter, + + (ParamFloat) _param_fw_p_lim_max, + (ParamFloat) _param_fw_p_lim_min, + + (ParamFloat) _param_fw_t_clmb_max, + (ParamFloat) _param_fw_t_hrate_ff, + (ParamFloat) _param_fw_t_h_error_tc, + (ParamFloat) _param_fw_t_I_gain_thr, + (ParamFloat) _param_fw_t_I_gain_pit, + (ParamFloat) _param_fw_t_ptch_damp, + (ParamFloat) _param_fw_t_rll2thr, + (ParamFloat) _param_fw_t_sink_max, + (ParamFloat) _param_fw_t_sink_min, + (ParamFloat) _param_fw_t_spd_omega, + (ParamFloat) _param_fw_t_spdweight, + (ParamFloat) _param_fw_t_tas_error_tc, + (ParamFloat) _param_fw_t_thr_damp, + (ParamFloat) _param_fw_t_vert_acc, + (ParamFloat) _param_ste_rate_time_const, + (ParamFloat) _param_tas_rate_time_const, + (ParamFloat) _param_seb_rate_ff, + (ParamFloat) _param_climbrate_target, + (ParamFloat) _param_sinkrate_target, + + (ParamFloat) _param_fw_thr_alt_scl, + (ParamFloat) _param_fw_thr_cruise, + (ParamFloat) _param_fw_thr_idle, + (ParamFloat) _param_fw_thr_lnd_max, + (ParamFloat) _param_fw_thr_max, + (ParamFloat) _param_fw_thr_min, + (ParamFloat) _param_fw_thr_slew_max, + + (ParamBool) _param_fw_posctl_inv_st, + + // external parameters + (ParamInt) _param_fw_arsp_mode, + + (ParamFloat) _param_fw_psp_off, + (ParamFloat) _param_fw_man_p_max, + (ParamFloat) _param_fw_man_r_max, + + (ParamFloat) _param_nav_loiter_rad, + + (ParamFloat) _takeoff_pitch_min + + ) + +}; + +#endif // FIXEDWINGPOSITIONCONTROL_HPP_ diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/prometheus_px4/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c new file mode 100644 index 0000000..4c6f6da --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -0,0 +1,827 @@ +/**************************************************************************** + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier + */ + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * L1 period + * + * This is the L1 distance and defines the tracking + * point ahead of the aircraft its following. + * A value of 18-25 meters works for most aircraft. Shorten + * slowly during tuning until response is sharp without oscillation. + * + * @unit m + * @min 12.0 + * @max 50.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); + +/** + * L1 damping + * + * Damping factor for L1 control. + * + * @min 0.6 + * @max 0.9 + * @decimal 2 + * @increment 0.05 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); + +/** + * L1 controller roll slew rate limit. + * + * The maxium change in roll angle setpoint per second. + * + * @unit deg/s + * @min 0 + * @increment 1 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f); + +/** + * Cruise throttle + * + * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f); + +/** + * Scale throttle by pressure change + * + * Automatically adjust throttle to account for decreased air density at higher altitudes. + * Start with a scale factor of 1.0 and adjust for different propulsion systems. + * + * When flying without airspeed sensor this will help to keep a constant performance over large altitude ranges. + * + * The default value of 0 will disable scaling. + * + * @min 0.0 + * @max 10.0 + * @decimal 1 + * @increment 0.1 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_ALT_SCL, 0.0f); + +/** + * Throttle max slew rate + * + * Maximum slew rate for the commanded throttle + * + * @min 0.0 + * @max 1.0 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); + +/** + * Negative pitch limit + * + * The minimum negative pitch the controller will output. + * + * @unit deg + * @min -60.0 + * @max 0.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); + +/** + * Positive pitch limit + * + * The maximum positive pitch the controller will output. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); + +/** + * Controller roll limit + * + * The maximum roll the controller will output. + * + * @unit deg + * @min 35.0 + * @max 65.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); + +/** + * Throttle limit max + * + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that + * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); + +/** + * Throttle limit min + * + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide + * some aerodynamic drag from a turning prop to improve the descent rate. + * + * For aircraft with internal combustion engine this parameter should be set + * for desired idle rpm. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); + +/** + * Idle throttle + * + * This is the minimum throttle while on the ground + * + * For aircraft with internal combustion engine this parameter should be set + * above desired idle rpm. + * + * @unit norm + * @min 0.0 + * @max 0.4 + * @decimal 2 + * @increment 0.01 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f); + +/** + * Throttle limit during landing below throttle limit altitude + * + * During the flare of the autonomous landing process, this value will be set + * as throttle limit when the aircraft altitude is below FW_LND_TLALT. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); + +/** + * Climbout Altitude difference + * + * If the altitude error exceeds this parameter, the system will climb out + * with maximum throttle and minimum airspeed until it is closer than this + * distance to the desired altitude. Mostly used for takeoff waypoints / modes. + * Set to 0 to disable climbout mode (not recommended). + * + * @unit m + * @min 0.0 + * @max 150.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f); + +/** + * Landing slope angle + * + * @unit deg + * @min 1.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); + +/** + * Minimum pitch during takeoff. + * + * @unit deg + * @min -5.0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); + +/** + * + * + * @unit m + * @min 1.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); + +/** + * Landing flare altitude (relative to landing altitude) + * + * @unit m + * @min 0.0 + * @max 25.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 3.0f); + +/** + * Landing throttle limit altitude (relative landing altitude) + * + * Default of -1.0 lets the system default to applying throttle + * limiting at 2/3 of the flare altitude. + * + * @unit m + * @min -1.0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); + +/** + * Landing heading hold horizontal distance. + * + * Set to 0 to disable heading hold. + * + * @unit m + * @min 0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); + +/** + * Use terrain estimate during landing. + * + * This is turned off by default and a waypoint or return altitude is normally used + * (or sea level for an arbitrary land position). + * + * @boolean + * @group FW L1 Control + */ +PARAM_DEFINE_INT32(FW_LND_USETER, 0); + +/** + * Early landing configuration deployment + * + * When disabled, the landing configuration (flaps, landing airspeed, etc.) is only activated + * on the final approach to landing. When enabled, it is already activated when entering the + * final loiter-down (loiter-to-alt) waypoint before the landing approach. This shifts the (often large) + * altitude and airspeed errors caused by the configuration change away from the ground such that + * these are not so critical. It also gives the controller enough time to adapt to the new + * configuration such that the landing approach starts with a cleaner initial state. + * + * @boolean + * + * @group FW L1 Control + */ +PARAM_DEFINE_INT32(FW_LND_EARLYCFG, 0); + +/** + * Flare, minimum pitch + * + * Minimum pitch during flare, a positive sign means nose up + * Applied once FW_LND_FLALT is reached + * + * @unit deg + * @min 0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f); + +/** + * Flare, maximum pitch + * + * Maximum pitch during flare, a positive sign means nose up + * Applied once FW_LND_FLALT is reached + * + * @unit deg + * @min 0 + * @max 45.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f); + +/** + * Min. airspeed scaling factor for landing + * + * Multiplying this factor with the minimum airspeed of the plane + * gives the target airspeed the landing approach. + * FW_AIRSPD_MIN * FW_LND_AIRSPD_SC + * + * @unit norm + * @min 1.0 + * @max 1.5 + * @decimal 2 + * @increment 0.01 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f); + +/** + * Altitude time constant factor for landing + * + * Set this parameter to less than 1.0 to make TECS react faster to altitude errors during + * landing than during normal flight (i.e. giving efficiency and low motor wear at + * high altitudes but control accuracy during landing). During landing, the TECS + * altitude time constant (FW_T_ALT_TC) is multiplied by this value. + * + * @unit + * @min 0.2 + * @max 1.0 + * @increment 0.1 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f); + + + +/* + * TECS parameters + * + */ + + +/** + * Minimum Airspeed (CAS) + * + * The minimal airspeed (calibrated airspeed) the user is able to command. + * Further, if the airspeed falls below this value, the TECS controller will try to + * increase airspeed more aggressively. + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); + +/** + * Maximum Airspeed (CAS) + * + * If the CAS (calibrated airspeed) is above this value, the TECS controller will try to decrease + * airspeed more aggressively. + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); + +/** + * Cruise Airspeed (CAS) + * + * The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active, + * this is the default airspeed setpoint that the controller will try to achieve if + * no other airspeed setpoint sources are present (e.g. through non-centered RC sticks). + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); + +/** + * Stall Airspeed (CAS) + * + * The stall airspeed (calibrated airspeed) of the vehicle. + * It is used for airspeed sensor failure detection and for the control + * surface scaling airspeed limits. + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f); + +/** + * Maximum climb rate + * + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * FW_THR_MAX reduced. + * + * @unit m/s + * @min 1.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); + +/** + * Minimum descent rate + * + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used + * to measure FW_T_CLMB_MAX. + * + * @unit m/s + * @min 1.0 + * @max 5.0 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); + +/** + * Maximum descent rate + * + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding + * the aircraft. + * + * @unit m/s + * @min 1.0 + * @max 15.0 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +/** + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * Increase to add damping to correct for oscillations in speed and height. + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.1f); + +/** + * Integrator gain throttle + * + * This is the integrator gain on the throttle part of the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. Set this value to zero to completely + * disable all integrator action. + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.05 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_I_GAIN_THR, 0.3f); + +/** + * Integrator gain pitch + * + * This is the integrator gain on the pitch part of the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. Set this value to zero to completely + * disable all integrator action. + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.05 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f); + +/** + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in m/s/s) + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover + * from under-speed conditions. + * + * @unit m/s^2 + * @min 1.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + +/** + * Complementary filter "omega" parameter for speed + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an + * improved airspeed estimate. Increasing this frequency weights the solution + * more towards use of the airspeed sensor, whilst reducing it weights the + * solution more towards use of the accelerometer data. + * + * @unit rad/s + * @min 1.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); + +/** + * Roll -> Throttle feedforward + * + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas + * inefficient low aspect-ratio models (eg delta wings) can use a higher value. + * + * @min 0.0 + * @max 20.0 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); + +/** + * Speed <--> Altitude priority + * + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * adjust its pitch angle to maintain airspeed, ignoring changes in height). + * + * @min 0.0 + * @max 2.0 + * @decimal 1 + * @increment 1.0 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); + +/** + * Pitch damping factor + * + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned + * properly. + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f); + +/** + * Altitude error time constant. + * + * @min 2.0 + * @decimal 2 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f); + +/** + * Height rate feed forward + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.3f); + +/** + * True airspeed error time constant. + * + * @min 2.0 + * @decimal 2 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 5.0f); + +/** + * Minimum groundspeed + * + * The controller will increase the commanded airspeed to maintain + * this minimum groundspeed to the next waypoint. + * + * @unit m/s + * @min 0.0 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); + +/** + * RC stick mapping fixed-wing. + * + * Set RC/joystick configuration for fixed-wing position and altitude controlled flight. + * + * @min 0 + * @max 1 + * @value 0 Normal stick configuration (airspeed on throttle stick, altitude on pitch stick) + * @value 1 Alternative stick configuration (altitude on throttle stick, airspeed on pitch stick) + * @group FW L1 Control + */ +PARAM_DEFINE_INT32(FW_POSCTL_INV_ST, 0); + +/** + * Specific total energy rate first order filter time constant. + * + * This filter is applied to the specific total energy rate used for throttle damping. + * + * @min 0.0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f); + + +/** + * True airspeed rate first order filter time constant. + * + * This filter is applied to the true airspeed rate. + * + * @min 0.0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.2f); + + +/** + * Specific total energy balance rate feedforward gain. + * + * + * @min 0.5 + * @max 3 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f); + +/** + * Default target climbrate. + * + * + * The default rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints. + * In manual modes this defines the maximum rate at which the altitude setpoint can be increased. + * + * + * @unit m/s + * @min 0.5 + * @max 15 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f); + +/** + * Default target sinkrate. + * + * + * The default rate at which the vehicle will sink in autonomous modes to achieve altitude setpoints. + * In manual modes this defines the maximum rate at which the altitude setpoint can be decreased. + * + * @unit m/s + * @min 0.5 + * @max 15 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f); diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt new file mode 100644 index 0000000..d7be8b5 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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_library(launchdetection + LaunchDetector.cpp + CatapultLaunchMethod.cpp +) \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp new file mode 100644 index 0000000..28f2fde --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 CatapultLaunchMethod.cpp + * Catapult Launch detection + * + * @author Thomas Gubler + * + */ + +#include "CatapultLaunchMethod.h" + +#include + +namespace launchdetection +{ + +CatapultLaunchMethod::CatapultLaunchMethod(ModuleParams *parent) : + ModuleParams(parent) +{ +} + +void CatapultLaunchMethod::update(const float dt, float accel_x) +{ + switch (state) { + case LAUNCHDETECTION_RES_NONE: + + /* Detect a acceleration that is longer and stronger as the minimum given by the params */ + if (accel_x > _param_laun_cat_a.get()) { + _integrator += dt; + + if (_integrator > _param_laun_cat_t.get()) { + if (_param_laun_cat_mdel.get() > 0.0f) { + state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle", + double(_param_laun_cat_mdel.get())); + + } else { + /* No motor delay set: go directly to enablemotors state */ + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + PX4_WARN("Launch detected: enablemotors (delay not activated)"); + } + } + + } else { + reset(); + } + + break; + + case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: + /* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is + * over to allow full throttle */ + _motorDelayCounter += dt; + + if (_motorDelayCounter > _param_laun_cat_mdel.get()) { + PX4_INFO("Launch detected: state enablemotors"); + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + break; + + default: + break; + + } +} + +LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const +{ + return state; +} + +void CatapultLaunchMethod::reset() +{ + _integrator = 0.0f; + _motorDelayCounter = 0.0f; + state = LAUNCHDETECTION_RES_NONE; +} + +float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) +{ + /* If motor is turned on do not impose the extra limit on maximum pitch */ + if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + return pitchMaxDefault; + + } else { + return _param_laun_cat_pmax.get(); + } +} + +} // namespace launchdetection diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h new file mode 100644 index 0000000..4458b0f --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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 CatapultLaunchMethod.h + * Catpult Launch detection + * + * @author Thomas Gubler + */ + +#ifndef CATAPULTLAUNCHMETHOD_H_ +#define CATAPULTLAUNCHMETHOD_H_ + +#include "LaunchMethod.h" + +#include +#include + +namespace launchdetection +{ + +class CatapultLaunchMethod : public LaunchMethod, public ModuleParams +{ +public: + CatapultLaunchMethod(ModuleParams *parent); + ~CatapultLaunchMethod() override = default; + + void update(const float dt, float accel_x) override; + LaunchDetectionResult getLaunchDetected() const override; + void reset() override; + float getPitchMax(float pitchMaxDefault) override; + +private: + float _integrator{0.0f}; + float _motorDelayCounter{0.0f}; + + LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_laun_cat_a, + (ParamFloat) _param_laun_cat_t, + (ParamFloat) _param_laun_cat_mdel, + (ParamFloat) _param_laun_cat_pmax /**< Upper pitch limit before throttle is turned on. + Can be used to make sure that the AC does not climb + too much while attached to a bungee */ + ) + +}; + +#endif /* CATAPULTLAUNCHMETHOD_H_ */ + +} // namespace launchdetection diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp new file mode 100644 index 0000000..c54fc9c --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 launchDetection.cpp + * Auto Detection for different launch methods (e.g. catapult) + * + * @author Thomas Gubler + */ + +#include "CatapultLaunchMethod.h" +#include "LaunchDetector.h" + +#include + +namespace launchdetection +{ + +LaunchDetector::LaunchDetector(ModuleParams *parent) : + ModuleParams(parent) +{ + /* init all detectors */ + _launchMethods[0] = new CatapultLaunchMethod(this); +} + +LaunchDetector::~LaunchDetector() +{ + delete _launchMethods[0]; +} + +void LaunchDetector::reset() +{ + /* Reset all detectors */ + for (const auto launchMethod : _launchMethods) { + launchMethod->reset(); + } + + /* Reset active launchdetector */ + _activeLaunchDetectionMethodIndex = -1; +} + +void LaunchDetector::update(const float dt, float accel_x) +{ + if (launchDetectionEnabled()) { + for (const auto launchMethod : _launchMethods) { + launchMethod->update(dt, accel_x); + } + } +} + +LaunchDetectionResult LaunchDetector::getLaunchDetected() +{ + if (launchDetectionEnabled()) { + if (_activeLaunchDetectionMethodIndex < 0) { + /* None of the active _launchmethods has detected a launch, check all _launchmethods */ + for (unsigned i = 0; i < (sizeof(_launchMethods) / sizeof(_launchMethods[0])); i++) { + if (_launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + PX4_INFO("selecting launchmethod %d", i); + _activeLaunchDetectionMethodIndex = i; // from now on only check this method + return _launchMethods[i]->getLaunchDetected(); + } + } + + } else { + return _launchMethods[_activeLaunchDetectionMethodIndex]->getLaunchDetected(); + } + } + + return LAUNCHDETECTION_RES_NONE; +} + +float LaunchDetector::getPitchMax(float pitchMaxDefault) +{ + if (!launchDetectionEnabled()) { + return pitchMaxDefault; + } + + /* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method, + * otherwise use the default limit */ + if (_activeLaunchDetectionMethodIndex < 0) { + if (sizeof(_launchMethods) / sizeof(LaunchMethod *) > 1) { + return pitchMaxDefault; + + } else { + return _launchMethods[0]->getPitchMax(pitchMaxDefault); + } + + } else { + return _launchMethods[_activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); + } +} + +} // namespace launchdetection diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h new file mode 100644 index 0000000..c96ccde --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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 LaunchDetector.h + * Auto Detection for different launch methods (e.g. catapult) + * + * @author Thomas Gubler + */ + +#ifndef LAUNCHDETECTOR_H +#define LAUNCHDETECTOR_H + +#include "LaunchMethod.h" +#include + +namespace launchdetection +{ + +class __EXPORT LaunchDetector : public ModuleParams +{ +public: + LaunchDetector(ModuleParams *parent); + ~LaunchDetector() override; + + LaunchDetector(const LaunchDetector &) = delete; + LaunchDetector operator=(const LaunchDetector &) = delete; + + void reset(); + + void update(const float dt, float accel_x); + LaunchDetectionResult getLaunchDetected(); + bool launchDetectionEnabled() { return _param_laun_all_on.get(); } + + /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */ + float getPitchMax(float pitchMaxDefault); + +private: + /* holds an index to the launchMethod in the array _launchMethods + * which detected a Launch. If no launchMethod has detected a launch yet the + * value is -1. Once one launchMethod has detected a launch only this + * method is checked for further advancing in the state machine + * (e.g. when to power up the motors) + */ + int _activeLaunchDetectionMethodIndex{-1}; + + LaunchMethod *_launchMethods[1]; + + DEFINE_PARAMETERS( + (ParamBool) _param_laun_all_on + ) +}; + +} // namespace launchdetection + +#endif // LAUNCHDETECTOR_H diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h new file mode 100644 index 0000000..ec0a7de --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 LaunchMethod.h + * Base class for different launch methods + * + * @author Thomas Gubler + */ + +#ifndef LAUNCHMETHOD_H_ +#define LAUNCHMETHOD_H_ + +namespace launchdetection +{ + +enum LaunchDetectionResult { + LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */ + LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should + control the attitude. However any motors should not throttle + up. For instance this is used to have a delay for the motor + when launching a fixed wing aircraft from a bungee */ + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, the controller should control + attitude and also throttle up the motors. */ +}; + +class LaunchMethod +{ +public: + virtual ~LaunchMethod() = default; + + virtual void update(const float dt, float accel_x) = 0; + virtual LaunchDetectionResult getLaunchDetected() const = 0; + virtual void reset() = 0; + + /* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */ + virtual float getPitchMax(float pitchMaxDefault) = 0; + +}; + +} // namespace launchdetection + +#endif /* LAUNCHMETHOD_H_ */ diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c new file mode 100644 index 0000000..1575cd0 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/launchdetection/launchdetection_params.c @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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 launchdetection_params.c + * + * Parameters for launchdetection + * + * @author Thomas Gubler + */ + +/* + * Catapult launch detection parameters, accessible via MAVLink + * + */ + +/** + * Launch detection + * + * @boolean + * @group FW Launch detection + */ +PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); + +/** + * Catapult accelerometer threshold. + * + * LAUN_CAT_A for LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @unit m/s^2 + * @min 0 + * @decimal 1 + * @increment 0.5 + * @group FW Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); + +/** + * Catapult time threshold. + * + * LAUN_CAT_A for LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @unit s + * @min 0.0 + * @max 5.0 + * @decimal 2 + * @increment 0.05 + * @group FW Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); + +/** + * Motor delay + * + * Delay between starting attitude control and powering up the throttle (giving throttle control to the controller) + * Before this timespan is up the throttle will be set to FW_THR_IDLE, set to 0 to deactivate + * + * @unit s + * @min 0.0 + * @max 10.0 + * @decimal 1 + * @increment 0.5 + * @group FW Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); + +/** + * Maximum pitch before the throttle is powered up (during motor delay phase) + * + * This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on. + * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep). + * + * @unit deg + * @min 0.0 + * @max 45.0 + * @decimal 1 + * @increment 0.5 + * @group FW Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f); diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/CMakeLists.txt b/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/CMakeLists.txt new file mode 100644 index 0000000..8217b10 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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_library(runway_takeoff + RunwayTakeoff.cpp +) \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp new file mode 100644 index 0000000..c9b9bfd --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * 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 RunwayTakeoff.cpp + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#include +#include +#include + +#include "RunwayTakeoff.h" +#include +#include + +using namespace time_literals; + +namespace runwaytakeoff +{ + +RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) : + ModuleParams(parent), + _state(), + _initialized(false), + _initialized_time(0), + _init_yaw(0), + _climbout(false) +{ +} + +void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat, double current_lon) +{ + _init_yaw = yaw; + _initialized = true; + _state = RunwayTakeoffState::THROTTLE_RAMP; + _initialized_time = now; + _climbout = true; // this is true until climbout is finished + _start_wp(0) = current_lat; + _start_wp(1) = current_lon; +} + +void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl, + double current_lat, double current_lon, orb_advert_t *mavlink_log_pub) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: + if (((now - _initialized_time) > (_param_rwto_ramp_time.get() * 1_s)) + || (airspeed > (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f))) { + + _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + } + + break; + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + if (airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get()) { + _state = RunwayTakeoffState::TAKEOFF; + mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached"); + } + + break; + + case RunwayTakeoffState::TAKEOFF: + if (alt_agl > _param_rwto_nav_alt.get()) { + _state = RunwayTakeoffState::CLIMBOUT; + + /* + * If we started in heading hold mode, move the navigation start WP to the current location now. + * The navigator will take this as starting point to navigate towards the takeoff WP. + */ + if (_param_rwto_hdg.get() == 0) { + _start_wp(0) = current_lat; + _start_wp(1) = current_lon; + } + + mavlink_log_info(mavlink_log_pub, "#Climbout"); + } + + break; + + case RunwayTakeoffState::CLIMBOUT: + if (alt_agl > _param_fw_clmbout_diff.get()) { + _climbout = false; + _state = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint"); + } + + break; + + default: + break; + } +} + +/* + * Returns true as long as we're below navigation altitude + */ +bool RunwayTakeoff::controlYaw() +{ + // keep controlling yaw directly until we start navigation + return _state < RunwayTakeoffState::CLIMBOUT; +} + +/* + * Returns pitch setpoint to use. + * + * Limited (parameter) as long as the plane is on runway. Otherwise + * use the one from TECS + */ +float RunwayTakeoff::getPitch(float tecsPitch) +{ + if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(_param_rwto_psp.get()); + } + + return tecsPitch; +} + +/* + * Returns the roll setpoint to use. + */ +float RunwayTakeoff::getRoll(float navigatorRoll) +{ + // until we have enough ground clearance, set roll to 0 + if (_state < RunwayTakeoffState::CLIMBOUT) { + return 0.0f; + } + + // allow some roll during climbout + else if (_state < RunwayTakeoffState::FLY) { + return math::constrain(navigatorRoll, + math::radians(-_param_rwto_max_roll.get()), + math::radians(_param_rwto_max_roll.get())); + } + + return navigatorRoll; +} + +/* + * Returns the yaw setpoint to use. + * + * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the + * runway. When it has enough ground clearance we start navigation towards WP. + */ +float RunwayTakeoff::getYaw(float navigatorYaw) +{ + if (_param_rwto_hdg.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + return _init_yaw; + + } else { + return navigatorYaw; + } +} + +/* + * Returns the throttle setpoint to use. + * + * Ramps up in the beginning, until it lifts off the runway it is set to + * parameter value, then it returns the TECS throttle. + */ +float RunwayTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: { + float throttle = ((now - _initialized_time) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get(); + return math::min(throttle, _param_rwto_max_thr.get()); + } + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + return _param_rwto_max_thr.get(); + + default: + return tecsThrottle; + } +} + +bool RunwayTakeoff::resetIntegrators() +{ + // reset integrators if we're still on runway + return _state < RunwayTakeoffState::TAKEOFF; +} + +/* + * Returns the minimum pitch for TECS to use. + * + * In climbout we either want what was set on the waypoint (sp_min) but at least + * the climbtout minimum pitch (parameter). + * Otherwise use the minimum that is enforced generally (parameter). + */ +float RunwayTakeoff::getMinPitch(float climbout_min, float min) +{ + if (_state < RunwayTakeoffState::FLY) { + return climbout_min; + + } else { + return min; + } +} + +/* + * Returns the maximum pitch for TECS to use. + * + * Limited by parameter (if set) until climbout is done. + */ +float RunwayTakeoff::getMaxPitch(float max) +{ + // use max pitch from parameter if set (> 0.1) + if (_state < RunwayTakeoffState::FLY && _param_rwto_max_pitch.get() > 0.1f) { + return _param_rwto_max_pitch.get(); + } + + else { + return max; + } +} + +void RunwayTakeoff::reset() +{ + _initialized = false; + _state = RunwayTakeoffState::THROTTLE_RAMP; +} + +} diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h new file mode 100644 index 0000000..796967d --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 RunwayTakeoff.h + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#ifndef RUNWAYTAKEOFF_H +#define RUNWAYTAKEOFF_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +enum RunwayTakeoffState { + THROTTLE_RAMP = 0, /**< ramping up throttle */ + CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */ + TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */ + CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */ + FLY = 4 /**< fly towards takeoff waypoint */ +}; + +class __EXPORT RunwayTakeoff : public ModuleParams +{ +public: + RunwayTakeoff(ModuleParams *parent); + ~RunwayTakeoff() = default; + + void init(const hrt_abstime &now, float yaw, double current_lat, double current_lon); + void update(const hrt_abstime &now, float airspeed, float alt_agl, double current_lat, double current_lon, + orb_advert_t *mavlink_log_pub); + + RunwayTakeoffState getState() { return _state; } + bool isInitialized() { return _initialized; } + + bool runwayTakeoffEnabled() { return _param_rwto_tkoff.get(); } + float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); } + float getInitYaw() { return _init_yaw; } + + bool controlYaw(); + bool climbout() { return _climbout; } + float getPitch(float tecsPitch); + float getRoll(float navigatorRoll); + float getYaw(float navigatorYaw); + float getThrottle(const hrt_abstime &now, float tecsThrottle); + bool resetIntegrators(); + float getMinPitch(float climbout_min, float min); + float getMaxPitch(float max); + const matrix::Vector2d &getStartWP() const { return _start_wp; }; + + void reset(); + +private: + /** state variables **/ + RunwayTakeoffState _state{THROTTLE_RAMP}; + bool _initialized{false}; + hrt_abstime _initialized_time{0}; + float _init_yaw{0.f}; + bool _climbout{false}; + matrix::Vector2d _start_wp; + + DEFINE_PARAMETERS( + (ParamBool) _param_rwto_tkoff, + (ParamInt) _param_rwto_hdg, + (ParamFloat) _param_rwto_nav_alt, + (ParamFloat) _param_rwto_max_thr, + (ParamFloat) _param_rwto_psp, + (ParamFloat) _param_rwto_max_pitch, + (ParamFloat) _param_rwto_max_roll, + (ParamFloat) _param_rwto_airspd_scl, + (ParamFloat) _param_rwto_ramp_time, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_clmbout_diff + ) + +}; + +} + +#endif // RUNWAYTAKEOFF_H diff --git a/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c b/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c new file mode 100644 index 0000000..5f165d0 --- /dev/null +++ b/src/prometheus_px4/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c @@ -0,0 +1,164 @@ +/**************************************************************************** + * + * 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 runway_takeoff_params.c + * + * Parameters for runway takeoff + * + * @author Andreas Antener + */ + +/** + * Runway takeoff with landing gear + * + * @boolean + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_TKOFF, 0); + +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @value 0 Airframe + * @value 1 Waypoint + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_HDG, 0); + +/** + * Altitude AGL at which we have enough ground clearance to allow some roll. + * + * Until RWTO_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see RWTO_HDG). This should be below + * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. + * + * @unit m + * @min 0.0 + * @max 100.0 + * @decimal 1 + * @increment 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * + * Can be used to test taxi on runway + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); + +/** + * Pitch setpoint during taxi / before takeoff airspeed is reached. + * + * A taildragger with steerable wheel might need to pitch up + * a little to keep its wheel on the ground before airspeed + * to takeoff is reached. + * + * @unit deg + * @min -10.0 + * @max 20.0 + * @decimal 1 + * @increment 0.5 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); + +/** + * Max pitch during takeoff. + * + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @decimal 1 + * @increment 0.5 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max roll during climbout. + * + * Roll is limited during climbout to ensure enough lift and prevents aggressive + * navigation before we're on a safe height. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @decimal 1 + * @increment 0.5 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); + +/** + * Min airspeed scaling factor for takeoff. + * + * Pitch up will be commanded when the following airspeed is reached: + * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + * + * @unit norm + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.01 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); + +/** + * Throttle ramp up time for runway takeoff + * + * @unit s + * @min 1.0 + * @max 15.0 + * @decimal 2 + * @increment 0.1 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f); diff --git a/src/prometheus_px4/src/modules/gyro_calibration/CMakeLists.txt b/src/prometheus_px4/src/modules/gyro_calibration/CMakeLists.txt new file mode 100644 index 0000000..5148eab --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_calibration/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__gyro_calibration + MAIN gyro_calibration + COMPILE_FLAGS + SRCS + GyroCalibration.cpp + GyroCalibration.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/gyro_calibration/GyroCalibration.cpp b/src/prometheus_px4/src/modules/gyro_calibration/GyroCalibration.cpp new file mode 100644 index 0000000..bac0733 --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_calibration/GyroCalibration.cpp @@ -0,0 +1,328 @@ +/**************************************************************************** + * + * 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 "GyroCalibration.hpp" + +#include + +using namespace time_literals; +using matrix::Vector3f; + +GyroCalibration::GyroCalibration() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ +} + +GyroCalibration::~GyroCalibration() +{ + perf_free(_loop_interval_perf); + perf_free(_calibration_updated_perf); +} + +bool GyroCalibration::init() +{ + ScheduleOnInterval(INTERVAL_US); + return true; +} + +void GyroCalibration::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_count(_loop_interval_perf); + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (armed != _armed) { + if (!_armed && armed) { + // disarmed -> armed: run at minimal rate until disarmed + ScheduleOnInterval(10_s); + + } else if (_armed && !armed) { + // armed -> disarmed: start running again + ScheduleOnInterval(INTERVAL_US); + } + + _armed = armed; + Reset(); + } + } + } + + if (_armed) { + // do nothing if armed + return; + } + + if (_vehicle_status_flags_sub.updated()) { + vehicle_status_flags_s vehicle_status_flags; + + if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) { + if (_system_calibrating != vehicle_status_flags.condition_calibration_enabled) { + Reset(); + _system_calibrating = vehicle_status_flags.condition_calibration_enabled; + } + } + } + + if (_system_calibrating) { + // do nothing if system is calibrating + return; + } + + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + + if (_parameter_update_sub.copy(¶m_update)) { + // minimize updates immediately following parameter changes + _last_calibration_update = param_update.timestamp; + } + + for (auto &cal : _gyro_calibration) { + cal.ParametersUpdate(); + } + } + + + // collect raw data from all available gyroscopes (sensor_gyro) + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + sensor_gyro_s sensor_gyro; + + while (_sensor_gyro_subs[gyro].update(&sensor_gyro)) { + if (PX4_ISFINITE(sensor_gyro.temperature)) { + if ((fabsf(_temperature[gyro] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[gyro])) { + PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", gyro, (double)_temperature[gyro], + (double)sensor_gyro.temperature); + + _temperature[gyro] = sensor_gyro.temperature; + + // reset all on any temperature change + Reset(); + } + + } else { + _temperature[gyro] = NAN; + } + + if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) { + const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()}; + _gyro_mean[gyro].update(val); + + } else { + // setting device id, reset all + _gyro_calibration[gyro].set_device_id(sensor_gyro.device_id); + Reset(); + } + } + } + + + // check all accelerometers for possible movement + for (int accel = 0; accel < _sensor_accel_subs.size(); accel++) { + sensor_accel_s sensor_accel; + + if (_sensor_accel_subs[accel].update(&sensor_accel)) { + const Vector3f acceleration{sensor_accel.x, sensor_accel.y, sensor_accel.z}; + + if ((acceleration - _acceleration[accel]).longerThan(0.5f)) { + // reset all on any change + PX4_DEBUG("accel %d changed, resetting all %.5f", accel, (double)(acceleration - _acceleration[accel]).length()); + + _acceleration[accel] = acceleration; + Reset(); + return; + + } else if (acceleration.longerThan(CONSTANTS_ONE_G * 1.3f)) { + Reset(); + return; + } + } + } + + + // check if sufficient data has been gathered to update calibration + bool sufficient_samples = false; + + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { + // periodically check variance + if ((_gyro_mean[gyro].count() % 100 == 0)) { + PX4_DEBUG("gyro %d (%" PRIu32 ") variance, [%.9f, %.9f, %.9f] %.9f", gyro, _gyro_calibration[gyro].device_id(), + (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2), + (double)_gyro_mean[gyro].variance().length()); + + if (_gyro_mean[gyro].variance().longerThan(0.001f)) { + // reset all + Reset(); + return; + } + } + + if (_gyro_mean[gyro].count() > 3000) { + sufficient_samples = true; + + } else { + sufficient_samples = false; + return; + } + } + } + + + // update calibrations for all available gyros + if (sufficient_samples && (hrt_elapsed_time(&_last_calibration_update) > 10_s)) { + bool calibration_updated = false; + + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { + _gyro_calibration[gyro].set_calibration_index(gyro); + + const Vector3f old_offset{_gyro_calibration[gyro].offset()}; + + if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) { + calibration_updated = true; + + PX4_INFO("gyro %d (%" PRIu32 ") updating calibration, [%.4f, %.4f, %.4f] -> [%.4f, %.4f, %.4f] %.1f°C", + gyro, _gyro_calibration[gyro].device_id(), + (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), + (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2), + (double)_temperature[gyro]); + + perf_count(_calibration_updated_perf); + } + } + } + + // save all calibrations + if (calibration_updated) { + bool param_save = false; + + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { + if (_gyro_calibration[gyro].ParametersSave()) { + param_save = true; + } + } + } + + if (param_save) { + param_notify_changes(); + _last_calibration_update = hrt_absolute_time(); + } + } + + Reset(); + } +} + +int GyroCalibration::task_spawn(int argc, char *argv[]) +{ + GyroCalibration *instance = new GyroCalibration(); + + 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 GyroCalibration::print_status() +{ + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { + PX4_INFO_RAW("gyro %d (%" PRIu32 "), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f°C (count %d)\n", + gyro, _gyro_calibration[gyro].device_id(), + (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2), + (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2), + (double)_temperature[gyro], _gyro_mean[gyro].count()); + } + } + + perf_print_counter(_loop_interval_perf); + perf_print_counter(_calibration_updated_perf); + return 0; +} + +int GyroCalibration::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int GyroCalibration::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Simple online gyroscope calibration. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("gyro_calibration", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int gyro_calibration_main(int argc, char *argv[]) +{ + return GyroCalibration::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/gyro_calibration/GyroCalibration.hpp b/src/prometheus_px4/src/modules/gyro_calibration/GyroCalibration.hpp new file mode 100644 index 0000000..1b2b925 --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_calibration/GyroCalibration.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class GyroCalibration : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + GyroCalibration(); + ~GyroCalibration() 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(); + + int print_status() override; + +private: + static constexpr hrt_abstime INTERVAL_US = 20000_us; + static constexpr int MAX_SENSORS = 4; + + void Run() override; + + void Reset() + { + for (auto &m : _gyro_mean) { + m.reset(); + } + } + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status}; + uORB::Subscription _vehicle_status_flags_sub{ORB_ID::vehicle_status_flags}; + + uORB::SubscriptionMultiArray _sensor_accel_subs{ORB_ID::sensor_accel}; + uORB::SubscriptionMultiArray _sensor_gyro_subs{ORB_ID::sensor_gyro}; + + calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {}; + math::WelfordMean _gyro_mean[MAX_SENSORS] {}; + float _temperature[MAX_SENSORS] {}; + + matrix::Vector3f _acceleration[MAX_SENSORS] {}; + + hrt_abstime _last_calibration_update{0}; + + bool _armed{false}; + bool _system_calibrating{false}; + + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _calibration_updated_perf{perf_alloc(PC_COUNT, MODULE_NAME": calibration updated")}; +}; diff --git a/src/prometheus_px4/src/modules/gyro_calibration/parameters.c b/src/prometheus_px4/src/modules/gyro_calibration/parameters.c new file mode 100644 index 0000000..dee9df9 --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_calibration/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** +* IMU gyro auto calibration enable. +* +* @boolean +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_GYRO_CAL_EN, 1); diff --git a/src/prometheus_px4/src/modules/gyro_fft/CMSIS_5/.clang-tidy b/src/prometheus_px4/src/modules/gyro_fft/CMSIS_5/.clang-tidy new file mode 100644 index 0000000..64e1b8f --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_fft/CMSIS_5/.clang-tidy @@ -0,0 +1,120 @@ +--- +Checks: '*, + -android*, + -bugprone-integer-division, + -cert-dcl50-cpp, + -cert-env33-c, + -cert-err34-c, + -cert-err58-cpp, + -cert-msc30-c, + -cert-msc50-cpp, + -cert-flp30-c, + -clang-analyzer-core.CallAndMessage, + -clang-analyzer-core.NullDereference, + -clang-analyzer-core.UndefinedBinaryOperatorResult, + -clang-analyzer-core.uninitialized.Assign, + -clang-analyzer-core.VLASize, + -clang-analyzer-cplusplus.NewDelete, + -clang-analyzer-cplusplus.NewDeleteLeaks, + -clang-analyzer-deadcode.DeadStores, + -clang-analyzer-optin.cplusplus.VirtualCall, + -clang-analyzer-optin.performance.Padding, + -clang-analyzer-security.FloatLoopCounter, + -clang-analyzer-security.insecureAPI.strcpy, + -clang-analyzer-unix.API, + -clang-analyzer-unix.cstring.BadSizeArg, + -clang-analyzer-unix.Malloc, + -clang-analyzer-unix.MallocSizeof, + -cppcoreguidelines-c-copy-assignment-signature, + -cppcoreguidelines-interfaces-global-init, + -cppcoreguidelines-no-malloc, + -cppcoreguidelines-owning-memory, + -cppcoreguidelines-pro-bounds-array-to-pointer-decay, + -cppcoreguidelines-pro-bounds-constant-array-index, + -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-pro-type-const-cast, + -cppcoreguidelines-pro-type-cstyle-cast, + -cppcoreguidelines-pro-type-member-init, + -cppcoreguidelines-pro-type-reinterpret-cast, + -cppcoreguidelines-pro-type-union-access, + -cppcoreguidelines-pro-type-vararg, + -cppcoreguidelines-special-member-functions, + -fuchsia-default-arguments, + -fuchsia-overloaded-operator, + -google-build-using-namespace, + -google-explicit-constructor, + -google-global-names-in-headers, + -google-readability-braces-around-statements, + -google-readability-casting, + -google-readability-function-size, + -google-readability-namespace-comments, + -google-readability-todo, + -google-runtime-int, + -google-runtime-references, + -hicpp-braces-around-statements, + -hicpp-deprecated-headers, + -hicpp-explicit-conversions, + -hicpp-function-size, + -hicpp-member-init, + -hicpp-no-array-decay, + -hicpp-no-assembler, + -hicpp-no-malloc, + -hicpp-signed-bitwise, + -hicpp-special-member-functions, + -hicpp-use-auto, + -hicpp-use-equals-default, + -hicpp-use-equals-delete, + -hicpp-use-override, + -hicpp-vararg, + -llvm-header-guard, + -llvm-include-order, + -llvm-namespace-comment, + -misc-incorrect-roundings, + -misc-macro-parentheses, + -misc-misplaced-widening-cast, + -misc-redundant-expression, + -misc-unconventional-assign-operator, + -misc-unused-parameters, + -modernize-deprecated-headers, + -modernize-loop-convert, + -modernize-pass-by-value, + -modernize-return-braced-init-list, + -modernize-use-auto, + -modernize-use-bool-literals, + -modernize-use-default-member-init, + -modernize-use-emplace, + -modernize-use-equals-default, + -modernize-use-equals-delete, + -modernize-use-override, + -modernize-use-using, + -performance-inefficient-string-concatenation, + -performance-unnecessary-value-param, + -readability-avoid-const-params-in-decls, + -readability-braces-around-statements, + -readability-container-size-empty, + -readability-delete-null-pointer, + -readability-else-after-return, + -readability-function-size, + -readability-implicit-bool-cast, + -readability-implicit-bool-conversion, + -readability-inconsistent-declaration-parameter-name, + -readability-named-parameter, + -readability-non-const-parameter, + -readability-redundant-control-flow, + -readability-redundant-declaration, + -readability-redundant-member-init, + -readability-simplify-boolean-expr, + -readability-static-accessed-through-instance, + -readability-static-definition-in-anonymous-namespace, + ' +WarningsAsErrors: '*' +CheckOptions: + - key: google-readability-braces-around-statements.ShortStatementLines + value: '1' + - key: google-readability-function-size.BranchThreshold + value: '600' + - key: google-readability-function-size.LineThreshold + value: '4000' + - key: google-readability-function-size.StatementThreshold + value: '4000' +... diff --git a/src/prometheus_px4/src/modules/gyro_fft/CMakeLists.txt b/src/prometheus_px4/src/modules/gyro_fft/CMakeLists.txt new file mode 100644 index 0000000..032109e --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_fft/CMakeLists.txt @@ -0,0 +1,75 @@ +############################################################################ +# +# Copyright (c) 2020-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. +# +############################################################################ + +set(CMSIS_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/CMSIS_5) +set(CMSIS_DSP ${CMSIS_ROOT}/CMSIS/DSP) + +if(${PX4_PLATFORM} MATCHES "NuttX") + add_compile_options(-DARM_MATH_DSP) +endif() + +add_compile_options($<$:-Wno-nested-externs>) + +px4_add_module( + MODULE modules__gyro_fft + MAIN gyro_fft + STACK_MAIN + 4096 + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + -DARM_ALL_FFT_TABLES + -DARM_MATH_LOOPUNROLL + INCLUDES + ${CMSIS_ROOT}/CMSIS/Core/Include + ${CMSIS_DSP}/Include + SRCS + GyroFFT.cpp + GyroFFT.hpp + + ${CMSIS_ROOT}/CMSIS/Core/Include/cmsis_compiler.h + ${CMSIS_ROOT}/CMSIS/Core/Include/cmsis_gcc.h + ${CMSIS_DSP}/Include/arm_common_tables.h + ${CMSIS_DSP}/Include/arm_const_structs.h + ${CMSIS_DSP}/Include/arm_math.h + ${CMSIS_DSP}/Source/BasicMathFunctions/arm_mult_q15.c + ${CMSIS_DSP}/Source/CommonTables/arm_common_tables.c + ${CMSIS_DSP}/Source/CommonTables/arm_const_structs.c + ${CMSIS_DSP}/Source/SupportFunctions/arm_float_to_q15.c + ${CMSIS_DSP}/Source/TransformFunctions/arm_bitreversal2.c + ${CMSIS_DSP}/Source/TransformFunctions/arm_cfft_q15.c + ${CMSIS_DSP}/Source/TransformFunctions/arm_cfft_radix4_q15.c + ${CMSIS_DSP}/Source/TransformFunctions/arm_rfft_init_q15.c + ${CMSIS_DSP}/Source/TransformFunctions/arm_rfft_q15.c + DEPENDS + px4_work_queue +) diff --git a/src/prometheus_px4/src/modules/gyro_fft/GyroFFT.cpp b/src/prometheus_px4/src/modules/gyro_fft/GyroFFT.cpp new file mode 100644 index 0000000..fafc7de --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_fft/GyroFFT.cpp @@ -0,0 +1,573 @@ +/**************************************************************************** + * + * Copyright (c) 2020, 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 "GyroFFT.hpp" + +#include +#include +#include + +using namespace matrix; + +GyroFFT::GyroFFT() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + for (int i = 0; i < MAX_NUM_PEAKS; i++) { + _sensor_gyro_fft.peak_frequencies_x[i] = NAN; + _sensor_gyro_fft.peak_frequencies_y[i] = NAN; + _sensor_gyro_fft.peak_frequencies_z[i] = NAN; + } +} + +GyroFFT::~GyroFFT() +{ + perf_free(_cycle_perf); + perf_free(_cycle_interval_perf); + perf_free(_fft_perf); + perf_free(_gyro_generation_gap_perf); + perf_free(_gyro_fifo_generation_gap_perf); + + delete[] _gyro_data_buffer_x; + delete[] _gyro_data_buffer_y; + delete[] _gyro_data_buffer_z; + delete[] _hanning_window; + delete[] _fft_input_buffer; + delete[] _fft_outupt_buffer; +} + +bool GyroFFT::init() +{ + bool buffers_allocated = false; + + // arm_rfft_init_q15(&_rfft_q15, _imu_gyro_fft_len, 0, 1) manually inlined to save flash + _rfft_q15.pTwiddleAReal = (q15_t *) realCoefAQ15; + _rfft_q15.pTwiddleBReal = (q15_t *) realCoefBQ15; + _rfft_q15.ifftFlagR = 0; + _rfft_q15.bitReverseFlagR = 1; + + switch (_param_imu_gyro_fft_len.get()) { + // case 128: + // buffers_allocated = AllocateBuffers<128>(); + // _rfft_q15.fftLenReal = 128; + // _rfft_q15.twidCoefRModifier = 64U; + // _rfft_q15.pCfft = &arm_cfft_sR_q15_len64; + // break; + + case 256: + buffers_allocated = AllocateBuffers<256>(); + _rfft_q15.fftLenReal = 256; + _rfft_q15.twidCoefRModifier = 32U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len128; + break; + + // case 512: + // buffers_allocated = AllocateBuffers<512>(); + // _rfft_q15.fftLenReal = 512; + // _rfft_q15.twidCoefRModifier = 16U; + // _rfft_q15.pCfft = &arm_cfft_sR_q15_len256; + // break; + + case 1024: + buffers_allocated = AllocateBuffers<1024>(); + _rfft_q15.fftLenReal = 1024; + _rfft_q15.twidCoefRModifier = 8U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len512; + break; + + // case 2048: + // buffers_allocated = AllocateBuffers<2048>(); + // _rfft_q15.fftLenReal = 2048; + // _rfft_q15.twidCoefRModifier = 4U; + // _rfft_q15.pCfft = &arm_cfft_sR_q15_len1024; + // break; + + case 4096: + buffers_allocated = AllocateBuffers<4096>(); + _rfft_q15.fftLenReal = 4096; + _rfft_q15.twidCoefRModifier = 2U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len2048; + break; + + // case 8192: + // buffers_allocated = AllocateBuffers<8192>(); + // _rfft_q15.fftLenReal = 8192; + // _rfft_q15.twidCoefRModifier = 1U; + // _rfft_q15.pCfft = &arm_cfft_sR_q15_len4096; + // break; + + default: + // otherwise default to 256 + PX4_ERR("Invalid IMU_GYRO_FFT_LEN=%" PRId32 ", resetting", _param_imu_gyro_fft_len.get()); + AllocateBuffers<256>(); + _param_imu_gyro_fft_len.set(256); + _param_imu_gyro_fft_len.commit(); + break; + } + + if (buffers_allocated) { + _imu_gyro_fft_len = _param_imu_gyro_fft_len.get(); + + // init Hanning window + for (int n = 0; n < _imu_gyro_fft_len; n++) { + const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (_imu_gyro_fft_len - 1))); + arm_float_to_q15(&hanning_value, &_hanning_window[n], 1); + } + + if (!SensorSelectionUpdate(true)) { + ScheduleDelayed(500_ms); + } + + return true; + } + + PX4_ERR("failed to allocate buffers"); + delete[] _gyro_data_buffer_x; + delete[] _gyro_data_buffer_y; + delete[] _gyro_data_buffer_z; + delete[] _hanning_window; + delete[] _fft_input_buffer; + delete[] _fft_outupt_buffer; + + return false; +} + +bool GyroFFT::SensorSelectionUpdate(bool force) +{ + if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) { + sensor_selection_s sensor_selection{}; + _sensor_selection_sub.copy(&sensor_selection); + + if ((sensor_selection.gyro_device_id != 0) && (_selected_sensor_device_id != sensor_selection.gyro_device_id)) { + // prefer sensor_gyro_fifo if available + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; + + if (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id) { + if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) { + _sensor_gyro_sub.unregisterCallback(); + _sensor_gyro_fifo_sub.set_required_updates(sensor_gyro_fifo_s::ORB_QUEUE_LENGTH - 1); + _selected_sensor_device_id = sensor_selection.gyro_device_id; + _gyro_fifo = true; + return true; + } + } + } + + // otherwise use sensor_gyro + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + + if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) { + if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) { + _sensor_gyro_fifo_sub.unregisterCallback(); + _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH - 1); + _selected_sensor_device_id = sensor_selection.gyro_device_id; + _gyro_fifo = false; + return true; + } + } + } + + PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", sensor_selection.gyro_device_id); + } + } + + return false; +} + +void GyroFFT::VehicleIMUStatusUpdate(bool force) +{ + if (_vehicle_imu_status_sub.updated() || force) { + vehicle_imu_status_s vehicle_imu_status; + + if (_vehicle_imu_status_sub.copy(&vehicle_imu_status)) { + // find corresponding vehicle_imu_status instance if the device_id doesn't match + if (vehicle_imu_status.gyro_device_id != _selected_sensor_device_id) { + + for (uint8_t imu_status = 0; imu_status < MAX_SENSOR_COUNT; imu_status++) { + uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_status}; + + if (imu_status_sub.copy(&vehicle_imu_status)) { + if (vehicle_imu_status.gyro_device_id == _selected_sensor_device_id) { + _vehicle_imu_status_sub.ChangeInstance(imu_status); + break; + } + } + } + } + + // update gyro sample rate + if ((vehicle_imu_status.gyro_device_id == _selected_sensor_device_id) && (vehicle_imu_status.gyro_rate_hz > 0)) { + if (_gyro_fifo) { + _gyro_sample_rate_hz = vehicle_imu_status.gyro_raw_rate_hz; + + } else { + _gyro_sample_rate_hz = vehicle_imu_status.gyro_rate_hz; + } + + return; + } + } + } +} + +// helper function used for frequency estimation +static float tau(float x) +{ + // tau(x) = 1/4 * log(3x^2 + 6x + 1) – sqrt(6)/24 * log((x + 1 – sqrt(2/3)) / (x + 1 + sqrt(2/3))) + float p1 = logf(3.f * powf(x, 2.f) + 6.f * x + 1.f); + float part1 = x + 1.f - sqrtf(2.f / 3.f); + float part2 = x + 1.f + sqrtf(2.f / 3.f); + float p2 = logf(part1 / part2); + return (0.25f * p1 - sqrtf(6.f) / 24.f * p2); +} + +float GyroFFT::EstimatePeakFrequency(q15_t fft[], int peak_index) +{ + if (peak_index > 2) { + // find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/) + float real[3] { (float)fft[peak_index - 2], (float)fft[peak_index], (float)fft[peak_index + 2] }; + float imag[3] { (float)fft[peak_index - 2 + 1], (float)fft[peak_index + 1], (float)fft[peak_index + 2 + 1] }; + + static constexpr int k = 1; + + const float divider = (real[k] * real[k] + imag[k] * imag[k]); + + // ap = (X[k + 1].r * X[k].r + X[k+1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i) + float ap = (real[k + 1] * real[k] + imag[k + 1] * imag[k]) / divider; + + // dp = -ap / (1 – ap) + float dp = -ap / (1.f - ap); + + // am = (X[k - 1].r * X[k].r + X[k – 1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i) + float am = (real[k - 1] * real[k] + imag[k - 1] * imag[k]) / divider; + + // dm = am / (1 – am) + float dm = am / (1.f - am); + + // d = (dp + dm) / 2 + tau(dp * dp) – tau(dm * dm) + float d = (dp + dm) / 2.f + tau(dp * dp) - tau(dm * dm); + + // k’ = k + d + float adjusted_bin = peak_index + d; + float peak_freq_adjusted = (_gyro_sample_rate_hz * adjusted_bin / (_imu_gyro_fft_len * 2.f)); + + return peak_freq_adjusted; + } + + return NAN; +} + +void GyroFFT::Run() +{ + if (should_exit()) { + _sensor_gyro_sub.unregisterCallback(); + _sensor_gyro_fifo_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + // backup schedule + ScheduleDelayed(500_ms); + + perf_begin(_cycle_perf); + perf_count(_cycle_interval_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + + const bool selection_updated = SensorSelectionUpdate(); + VehicleIMUStatusUpdate(selection_updated); + + if (_gyro_fifo) { + // run on sensor gyro fifo updates + sensor_gyro_fifo_s sensor_gyro_fifo; + + while (_sensor_gyro_fifo_sub.update(&sensor_gyro_fifo)) { + if (_sensor_gyro_fifo_sub.get_last_generation() != _gyro_last_generation + 1) { + // force reset if we've missed a sample + _fft_buffer_index[0] = 0; + _fft_buffer_index[1] = 0; + _fft_buffer_index[2] = 0; + + perf_count(_gyro_fifo_generation_gap_perf); + } + + _gyro_last_generation = _sensor_gyro_fifo_sub.get_last_generation(); + + if (fabsf(sensor_gyro_fifo.scale - _fifo_last_scale) > FLT_EPSILON) { + // force reset if scale has changed + _fft_buffer_index[0] = 0; + _fft_buffer_index[1] = 0; + _fft_buffer_index[2] = 0; + + _fifo_last_scale = sensor_gyro_fifo.scale; + } + + int16_t *input[] {sensor_gyro_fifo.x, sensor_gyro_fifo.y, sensor_gyro_fifo.z}; + Update(sensor_gyro_fifo.timestamp_sample, input, sensor_gyro_fifo.samples); + } + + } else { + // run on sensor gyro fifo updates + sensor_gyro_s sensor_gyro; + + while (_sensor_gyro_sub.update(&sensor_gyro)) { + if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { + // force reset if we've missed a sample + _fft_buffer_index[0] = 0; + _fft_buffer_index[1] = 0; + _fft_buffer_index[2] = 0; + + perf_count(_gyro_generation_gap_perf); + } + + _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); + + const float gyro_scale = math::radians(1000.f); // arbitrary scaling float32 rad/s -> raw int16 + int16_t gyro_x[1] {(int16_t)roundf(sensor_gyro.x * gyro_scale)}; + int16_t gyro_y[1] {(int16_t)roundf(sensor_gyro.y * gyro_scale)}; + int16_t gyro_z[1] {(int16_t)roundf(sensor_gyro.z * gyro_scale)}; + + int16_t *input[] {gyro_x, gyro_y, gyro_z}; + Update(sensor_gyro.timestamp_sample, input, 1); + } + } + + perf_end(_cycle_perf); +} + +void GyroFFT::Update(const hrt_abstime ×tamp_sample, int16_t *input[], uint8_t N) +{ + bool publish = false; + bool fft_updated = false; + const float resolution_hz = _gyro_sample_rate_hz / _imu_gyro_fft_len; + q15_t *gyro_data_buffer[] {_gyro_data_buffer_x, _gyro_data_buffer_y, _gyro_data_buffer_z}; + + for (int axis = 0; axis < 3; axis++) { + int &buffer_index = _fft_buffer_index[axis]; + + for (int n = 0; n < N; n++) { + if (buffer_index < _imu_gyro_fft_len) { + // convert int16_t -> q15_t (scaling isn't relevant) + gyro_data_buffer[axis][buffer_index] = input[axis][n] / 2; + buffer_index++; + } + + // if we have enough samples begin processing, but only one FFT per cycle + if ((buffer_index >= _imu_gyro_fft_len) && !fft_updated) { + perf_begin(_fft_perf); + arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _imu_gyro_fft_len); + arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer); + perf_end(_fft_perf); + + fft_updated = true; + + // sum total energy across all used buckets for SNR + float bin_mag_sum = 0; + + for (uint16_t bucket_index = 2; bucket_index < (_imu_gyro_fft_len / 2); bucket_index = bucket_index + 2) { + const float freq_hz = (bucket_index / 2) * resolution_hz; + + if (freq_hz >= _param_imu_gyro_fft_max.get()) { + break; + } + + const float real = _fft_outupt_buffer[bucket_index]; + const float imag = _fft_outupt_buffer[bucket_index + 1]; + + const float fft_magnitude_squared = real * real + imag * imag; + + bin_mag_sum += fft_magnitude_squared; + } + + _sensor_gyro_fft.total_energy[axis] = bin_mag_sum; + + + bool peaks_detected = false; + float peaks_magnitude[MAX_NUM_PEAKS] {}; + int peak_index[MAX_NUM_PEAKS] {}; + + // start at 2 to skip DC + // output is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1] + for (uint16_t bucket_index = 2; bucket_index < (_imu_gyro_fft_len / 2); bucket_index = bucket_index + 2) { + const float freq_hz = (bucket_index / 2) * resolution_hz; + + if (freq_hz >= _param_imu_gyro_fft_max.get()) { + break; + } + + const float real = _fft_outupt_buffer[bucket_index]; + const float imag = _fft_outupt_buffer[bucket_index + 1]; + + const float fft_magnitude_squared = real * real + imag * imag; + + float snr = 10.f * log10f((_imu_gyro_fft_len - 1) * fft_magnitude_squared / (bin_mag_sum - fft_magnitude_squared)); + + static constexpr float MIN_SNR = 10.f; // TODO: configurable? + + if (snr > MIN_SNR) { + for (int i = 0; i < MAX_NUM_PEAKS; i++) { + if (fft_magnitude_squared > peaks_magnitude[i]) { + peaks_magnitude[i] = fft_magnitude_squared; + peak_index[i] = bucket_index; + peaks_detected = true; + break; + } + } + } + } + + if (peaks_detected) { + float *peak_frequencies[] {_sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z}; + float *peak_magnitude[] {_sensor_gyro_fft.peak_magnitude_x, _sensor_gyro_fft.peak_magnitude_y, _sensor_gyro_fft.peak_magnitude_z}; + + int num_peaks_found = 0; + + for (int i = 0; i < MAX_NUM_PEAKS; i++) { + if ((peak_index[i] > 0) && (peak_index[i] < _imu_gyro_fft_len) && (peaks_magnitude[i] > 0)) { + const float freq = EstimatePeakFrequency(_fft_outupt_buffer, peak_index[i]); + + if (PX4_ISFINITE(freq) && freq >= _param_imu_gyro_fft_min.get() && freq <= _param_imu_gyro_fft_max.get()) { + + if (!PX4_ISFINITE(peak_frequencies[axis][num_peaks_found]) + || (fabsf(peak_frequencies[axis][num_peaks_found] - freq) > 0.01f)) { + + publish = true; + _sensor_gyro_fft.timestamp_sample = timestamp_sample; + } + + peak_frequencies[axis][num_peaks_found] = freq; + peak_magnitude[axis][num_peaks_found] = peaks_magnitude[i]; + + num_peaks_found++; + } + } + } + + // mark remaining slots empty + for (int i = num_peaks_found; i < MAX_NUM_PEAKS; i++) { + peak_frequencies[axis][i] = NAN; + peak_magnitude[axis][i] = NAN; + } + } + + // reset + // shift buffer (3/4 overlap) + const int overlap_start = _imu_gyro_fft_len / 4; + memmove(&gyro_data_buffer[axis][0], &gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3); + buffer_index = overlap_start * 3; + } + } + } + + if (publish) { + _sensor_gyro_fft.device_id = _selected_sensor_device_id; + _sensor_gyro_fft.sensor_sample_rate_hz = _gyro_sample_rate_hz; + _sensor_gyro_fft.resolution_hz = resolution_hz; + _sensor_gyro_fft.timestamp = hrt_absolute_time(); + _sensor_gyro_fft_pub.publish(_sensor_gyro_fft); + + publish = false; + } +} + +int GyroFFT::task_spawn(int argc, char *argv[]) +{ + GyroFFT *instance = new GyroFFT(); + + 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 GyroFFT::print_status() +{ + PX4_INFO("gyro sample rate: %.3f Hz", (double)_gyro_sample_rate_hz); + perf_print_counter(_cycle_perf); + perf_print_counter(_cycle_interval_perf); + perf_print_counter(_fft_perf); + perf_print_counter(_gyro_generation_gap_perf); + perf_print_counter(_gyro_fifo_generation_gap_perf); + return 0; +} + +int GyroFFT::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int GyroFFT::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("gyro_fft", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int gyro_fft_main(int argc, char *argv[]) +{ + return GyroFFT::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/gyro_fft/GyroFFT.hpp b/src/prometheus_px4/src/modules/gyro_fft/GyroFFT.hpp new file mode 100644 index 0000000..29705d5 --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_fft/GyroFFT.hpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "arm_math.h" +#include "arm_const_structs.h" + +using namespace time_literals; + +class GyroFFT : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + GyroFFT(); + ~GyroFFT() 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); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + bool init(); + +private: + float EstimatePeakFrequency(q15_t fft[], int peak_index); + void Run() override; + bool SensorSelectionUpdate(bool force = false); + void Update(const hrt_abstime ×tamp_sample, int16_t *input[], uint8_t N); + void VehicleIMUStatusUpdate(bool force = false); + + template + bool AllocateBuffers() + { + _gyro_data_buffer_x = new q15_t[N]; + _gyro_data_buffer_y = new q15_t[N]; + _gyro_data_buffer_z = new q15_t[N]; + _hanning_window = new q15_t[N]; + _fft_input_buffer = new q15_t[N]; + _fft_outupt_buffer = new q15_t[N * 2]; + + return (_gyro_data_buffer_x && _gyro_data_buffer_y && _gyro_data_buffer_z + && _hanning_window + && _fft_input_buffer + && _fft_outupt_buffer); + } + + static constexpr int MAX_SENSOR_COUNT = 4; + + static constexpr int MAX_NUM_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof( + sensor_gyro_fft_s::peak_frequencies_x[0]); + + uORB::Publication _sensor_gyro_fft_pub{ORB_ID(sensor_gyro_fft)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; + uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)}; + + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)}; + uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + perf_counter_t _fft_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": FFT")}; + perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; + perf_counter_t _gyro_fifo_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro FIFO data gap")}; + + uint32_t _selected_sensor_device_id{0}; + + bool _gyro_fifo{false}; + + arm_rfft_instance_q15 _rfft_q15; + + q15_t *_gyro_data_buffer_x{nullptr}; + q15_t *_gyro_data_buffer_y{nullptr}; + q15_t *_gyro_data_buffer_z{nullptr}; + q15_t *_hanning_window{nullptr}; + q15_t *_fft_input_buffer{nullptr}; + q15_t *_fft_outupt_buffer{nullptr}; + + float _gyro_sample_rate_hz{8000}; // 8 kHz default + + float _fifo_last_scale{0}; + + int _fft_buffer_index[3] {}; + + unsigned _gyro_last_generation{0}; + + sensor_gyro_fft_s _sensor_gyro_fft{}; + + int32_t _imu_gyro_fft_len{256}; + + DEFINE_PARAMETERS( + (ParamInt) _param_imu_gyro_fft_len, + (ParamFloat) _param_imu_gyro_fft_min, + (ParamFloat) _param_imu_gyro_fft_max + ) +}; diff --git a/src/prometheus_px4/src/modules/gyro_fft/parameters.c b/src/prometheus_px4/src/modules/gyro_fft/parameters.c new file mode 100644 index 0000000..9062b9d --- /dev/null +++ b/src/prometheus_px4/src/modules/gyro_fft/parameters.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** +* IMU gyro FFT enable. +* +* @boolean +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_GYRO_FFT_EN, 0); + +/** +* IMU gyro FFT minimum frequency. +* +* @min 1 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 32.f); + +/** +* IMU gyro FFT maximum frequency. +* +* @min 1 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 192.f); + +/** +* IMU gyro FFT length. +* +* @value 256 256 +* @value 1024 1024 +* @value 4096 4096 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_GYRO_FFT_LEN, 1024); diff --git a/src/prometheus_px4/src/modules/land_detector/AirshipLandDetector.cpp b/src/prometheus_px4/src/modules/land_detector/AirshipLandDetector.cpp new file mode 100644 index 0000000..9a7ee86 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/AirshipLandDetector.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 AirshipLandDetector.cpp + * Land detection algorithm for airships + * + * @author Anton Erasmus + */ + +#include "AirshipLandDetector.h" + +namespace land_detector +{ + +bool AirshipLandDetector::_get_ground_contact_state() +{ + return false; +} + +bool AirshipLandDetector::_get_landed_state() +{ + + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { + return true; // If Landing has been requested then say we have landed. + + } else { + return !_armed; // If we are armed we are not landed. + } +} + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/AirshipLandDetector.h b/src/prometheus_px4/src/modules/land_detector/AirshipLandDetector.h new file mode 100644 index 0000000..abbd693 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/AirshipLandDetector.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 AirshipLandDetector.h + * Land detection implementation for airships. + * + * @author Anton Erasmus + */ + +#pragma once + +#include "LandDetector.h" + +namespace land_detector +{ + +class AirshipLandDetector : public LandDetector +{ +public: + AirshipLandDetector() = default; + ~AirshipLandDetector() override = default; + +protected: + bool _get_ground_contact_state() override; + bool _get_landed_state() override; + void _set_hysteresis_factor(const int factor) override {}; +}; + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/CMakeLists.txt b/src/prometheus_px4/src/modules/land_detector/CMakeLists.txt new file mode 100644 index 0000000..632c90d --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# 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__land_detector + MAIN land_detector + COMPILE_FLAGS + SRCS + land_detector_main.cpp + LandDetector.cpp + MulticopterLandDetector.cpp + FixedwingLandDetector.cpp + VtolLandDetector.cpp + RoverLandDetector.cpp + AirshipLandDetector.cpp + DEPENDS + hysteresis + ) + diff --git a/src/prometheus_px4/src/modules/land_detector/FixedwingLandDetector.cpp b/src/prometheus_px4/src/modules/land_detector/FixedwingLandDetector.cpp new file mode 100644 index 0000000..845e6cd --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/FixedwingLandDetector.cpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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 FixedwingLandDetector.cpp + * + * @author Johan Jansen + * @author Lorenz Meier + * @author Julian Oes + */ + +#include "FixedwingLandDetector.h" + +namespace land_detector +{ + +FixedwingLandDetector::FixedwingLandDetector() +{ + // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). + _landed_hysteresis.set_hysteresis_time_from(false, LANDED_TRIGGER_TIME_US); + _landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US); +} + +bool FixedwingLandDetector::_get_landed_state() +{ + // Only trigger flight conditions if we are armed. + if (!_armed) { + return true; + } + + bool landDetected = false; + + if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { + + // Horizontal velocity complimentary filter. + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + + _vehicle_local_position.vy * _vehicle_local_position.vy); + + if (PX4_ISFINITE(val)) { + _velocity_xy_filtered = val; + } + + // Vertical velocity complimentary filter. + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicle_local_position.vz); + + if (PX4_ISFINITE(val)) { + _velocity_z_filtered = val; + } + + airspeed_validated_s airspeed_validated{}; + _airspeed_validated_sub.copy(&airspeed_validated); + + // set _airspeed_filtered to 0 if airspeed data is invalid + if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) { + _airspeed_filtered = 0.0f; + + } else { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s; + } + + // A leaking lowpass prevents biases from building up, but + // gives a mostly correct response for short impulses. + const float acc_hor = matrix::Vector2f(_acceleration).norm(); + _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; + + // Crude land detector for fixedwing. + landDetected = _airspeed_filtered < _param_lndfw_airspd.get() + && _velocity_xy_filtered < _param_lndfw_vel_xy_max.get() + && _velocity_z_filtered < _param_lndfw_vel_z_max.get() + && _xy_accel_filtered < _param_lndfw_xyaccel_max.get(); + + } else { + // Control state topic has timed out and we need to assume we're landed. + landDetected = true; + } + + return landDetected; +} + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/FixedwingLandDetector.h b/src/prometheus_px4/src/modules/land_detector/FixedwingLandDetector.h new file mode 100644 index 0000000..b8ca63a --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/FixedwingLandDetector.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 FixedwingLandDetector.h + * Land detector implementation for fixedwing. + * + * @author Johan Jansen + * @author Morten Lysgaard + * @author Julian Oes + */ + +#pragma once + +#include +#include + +#include "LandDetector.h" + +using namespace time_literals; + +namespace land_detector +{ + +class FixedwingLandDetector final : public LandDetector +{ +public: + FixedwingLandDetector(); + ~FixedwingLandDetector() override = default; + +protected: + + bool _get_landed_state() override; + void _set_hysteresis_factor(const int factor) override {}; + +private: + + /** Time in us that landing conditions have to hold before triggering a land. */ + static constexpr hrt_abstime LANDED_TRIGGER_TIME_US = 2_s; + static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us; + + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + + float _airspeed_filtered{0.0f}; + float _velocity_xy_filtered{0.0f}; + float _velocity_z_filtered{0.0f}; + float _xy_accel_filtered{0.0f}; + + DEFINE_PARAMETERS_CUSTOM_PARENT( + LandDetector, + (ParamFloat) _param_lndfw_xyaccel_max, + (ParamFloat) _param_lndfw_airspd, + (ParamFloat) _param_lndfw_vel_xy_max, + (ParamFloat) _param_lndfw_vel_z_max + ); +}; + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/LandDetector.cpp b/src/prometheus_px4/src/modules/land_detector/LandDetector.cpp new file mode 100644 index 0000000..34e0f49 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/LandDetector.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * 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 LandDetector.cpp + * + * @author Johan Jansen + * @author Julian Oes + */ + +#include "LandDetector.h" + +using namespace time_literals; + +namespace land_detector +{ + +LandDetector::LandDetector() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{} + +LandDetector::~LandDetector() +{ + perf_free(_cycle_perf); +} + +void LandDetector::start() +{ + ScheduleDelayed(50_ms); + _vehicle_local_position_sub.registerCallback(); +} + +void LandDetector::Run() +{ + // push backup schedule + ScheduleDelayed(50_ms); + + perf_begin(_cycle_perf); + + if (_parameter_update_sub.updated() || (_land_detected.timestamp == 0)) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + _update_params(); + + _total_flight_time = static_cast(_param_total_flight_time_high.get()) << 32; + _total_flight_time |= static_cast(_param_total_flight_time_low.get()); + } + + actuator_armed_s actuator_armed; + + if (_actuator_armed_sub.update(&actuator_armed)) { + _armed = actuator_armed.armed; + } + + vehicle_acceleration_s vehicle_acceleration; + + if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) { + _acceleration = matrix::Vector3f{vehicle_acceleration.xyz}; + } + + _vehicle_local_position_sub.update(&_vehicle_local_position); + _vehicle_status_sub.update(&_vehicle_status); + + _update_topics(); + + if (!_dist_bottom_is_observable) { + // we consider the distance to the ground observable if the system is using a range sensor + _dist_bottom_is_observable = _vehicle_local_position.dist_bottom_sensor_bitfield & + vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE; + } + + // Increase land detection time if not close to ground + if (_dist_bottom_is_observable && !_vehicle_local_position.dist_bottom_valid) { + _set_hysteresis_factor(3); + + } else { + _set_hysteresis_factor(1); + } + + const hrt_abstime now_us = hrt_absolute_time(); + + _freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us); + _ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us); + _maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us); + _landed_hysteresis.set_state_and_update(_get_landed_state(), now_us); + _ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us); + + const bool freefallDetected = _freefall_hysteresis.get_state(); + const bool ground_contactDetected = _ground_contact_hysteresis.get_state(); + const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state(); + const bool landDetected = _landed_hysteresis.get_state(); + const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : (float)INFINITY; + const bool in_ground_effect = _ground_effect_hysteresis.get_state(); + + // publish at 1 Hz, very first time, or when the result has changed + if ((hrt_elapsed_time(&_land_detected.timestamp) >= 1_s) || + (_land_detected.landed != landDetected) || + (_land_detected.freefall != freefallDetected) || + (_land_detected.maybe_landed != maybe_landedDetected) || + (_land_detected.ground_contact != ground_contactDetected) || + (_land_detected.in_ground_effect != in_ground_effect) || + (fabsf(_land_detected.alt_max - alt_max) > FLT_EPSILON)) { + + if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */ + // We did take off + _takeoff_time = now_us; + } + + _land_detected.landed = landDetected; + _land_detected.freefall = freefallDetected; + _land_detected.maybe_landed = maybe_landedDetected; + _land_detected.ground_contact = ground_contactDetected; + _land_detected.alt_max = alt_max; + _land_detected.in_ground_effect = in_ground_effect; + _land_detected.in_descend = _get_in_descend(); + _land_detected.has_low_throttle = _get_has_low_throttle(); + _land_detected.horizontal_movement = _get_horizontal_movement(); + _land_detected.vertical_movement = _get_vertical_movement(); + _land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check(); + _land_detected.timestamp = hrt_absolute_time(); + _vehicle_land_detected_pub.publish(_land_detected); + } + + // set the flight time when disarming (not necessarily when landed, because all param changes should + // happen on the same event and it's better to set/save params while not in armed state) + if (_takeoff_time != 0 && !_armed && _previous_armed_state) { + _total_flight_time += now_us - _takeoff_time; + _takeoff_time = 0; + + uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff; + + _param_total_flight_time_high.set(flight_time); + _param_total_flight_time_high.commit_no_notification(); + + flight_time = _total_flight_time & 0xffffffff; + + _param_total_flight_time_low.set(flight_time); + _param_total_flight_time_low.commit_no_notification(); + } + + _previous_armed_state = _armed; + + perf_end(_cycle_perf); + + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } +} + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/LandDetector.h b/src/prometheus_px4/src/modules/land_detector/LandDetector.h new file mode 100644 index 0000000..09b502d --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/LandDetector.h @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 LandDetector.h + * Land detector interface for multicopter, fixedwing and VTOL implementations. + * + * @author Johan Jansen + * @author Julian Oes + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace land_detector +{ + +class LandDetector : public ModuleBase, ModuleParams, px4::ScheduledWorkItem +{ +public: + LandDetector(); + virtual ~LandDetector(); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + /** + * Get the work queue going. + */ + void start(); + + static int task_spawn(int argc, char *argv[]); + +protected: + + /** + * Updates parameters. + */ + virtual void _update_params() {}; + + /** + * Updates subscribed uORB topics. + */ + virtual void _update_topics() {}; + + /** + * @return true if UAV is in a landed state. + */ + virtual bool _get_landed_state() = 0; + + /** + * @return true if UAV is in almost landed state + */ + virtual bool _get_maybe_landed_state() { return false; } + + /** + * @return true if UAV is touching ground but not landed + */ + virtual bool _get_ground_contact_state() { return false; } + + /** + * @return true if UAV is in free-fall state. + */ + virtual bool _get_freefall_state() { return false; } + + /** + * @return maximum altitude that can be reached + */ + virtual float _get_max_altitude() { return INFINITY; } + + /** + * @return true if vehicle could be in ground effect (close to ground) + */ + virtual bool _get_ground_effect_state() { return false; } + + virtual bool _get_in_descend() { return false; } + virtual bool _get_has_low_throttle() { return false; } + virtual bool _get_horizontal_movement() { return false; } + virtual bool _get_vertical_movement() { return false; } + virtual bool _get_close_to_ground_or_skipped_check() { return false; } + virtual void _set_hysteresis_factor(const int factor) = 0; + + systemlib::Hysteresis _freefall_hysteresis{false}; + systemlib::Hysteresis _landed_hysteresis{true}; + systemlib::Hysteresis _maybe_landed_hysteresis{true}; + systemlib::Hysteresis _ground_contact_hysteresis{true}; + systemlib::Hysteresis _ground_effect_hysteresis{false}; + + vehicle_local_position_s _vehicle_local_position{}; + vehicle_status_s _vehicle_status{}; + + matrix::Vector3f _acceleration{}; + + bool _armed{false}; + bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state + bool _dist_bottom_is_observable{false}; + +private: + void Run() override; + + vehicle_land_detected_s _land_detected = { + .timestamp = 0, + .alt_max = -1.0f, + .freefall = false, + .ground_contact = true, + .maybe_landed = true, + .landed = true, + }; + + hrt_abstime _takeoff_time{0}; + hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + uORB::Publication _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; + + DEFINE_PARAMETERS_CUSTOM_PARENT( + ModuleParams, + (ParamInt) _param_total_flight_time_high, + (ParamInt) _param_total_flight_time_low + ); +}; + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/MulticopterLandDetector.cpp b/src/prometheus_px4/src/modules/land_detector/MulticopterLandDetector.cpp new file mode 100644 index 0000000..15921ef --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/MulticopterLandDetector.cpp @@ -0,0 +1,371 @@ +/**************************************************************************** + * + * 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 MulticopterLandDetector.cpp + * + *The MC land-detector goes through 3 states before it will detect landed: + * + *State 1 (=ground_contact): + *ground_contact is detected once the vehicle is not moving along the NED-z direction and has + *a thrust value below 0.3 of the thrust_range (thrust_hover - thrust_min). The condition has to be true + *for GROUND_CONTACT_TRIGGER_TIME_US in order to detect ground_contact + * + *State 2 (=maybe_landed): + *maybe_landed can only occur if the internal ground_contact hysteresis state is true. maybe_landed criteria requires to have no motion in x and y, + *no rotation and a thrust below 0.1 of the thrust_range (thrust_hover - thrust_min). In addition, the mc_pos_control turns off the thrust_sp in + *body frame along x and y which helps to detect maybe_landed. The criteria for maybe_landed needs to be true for MAYBE_LAND_DETECTOR_TRIGGER_TIME_US. + * + *State 3 (=landed) + *landed can only be detected if maybe_landed is true for LAND_DETECTOR_TRIGGER_TIME_US. No farther criteria is tested, but the mc_pos_control goes into + *idle (thrust_sp = 0) which helps to detect landed. By doing this the thrust-criteria of State 2 will always be met, however the remaining criteria of no rotation and no motion still + *have to be valid. + + *It is to note that if one criteria is not met, then vehicle exits the state directly without blocking. + * + *If the land-detector does not detect ground_contact, then the vehicle is either flying or falling, where free fall detection heavily relies + *on the acceleration. TODO: verify that free fall is reliable + * + * @author Johan Jansen + * @author Morten Lysgaard + * @author Julian Oes + */ + +#include +#include +#include + +#include "MulticopterLandDetector.h" + +using matrix::Vector2f; +using matrix::Vector3f; + +namespace land_detector +{ + +MulticopterLandDetector::MulticopterLandDetector() +{ + _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); + _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); + _paramHandle.minThrottle = param_find("MPC_THR_MIN"); + _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); + _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); +} + +void MulticopterLandDetector::_update_topics() +{ + actuator_controls_s actuator_controls; + + if (_actuator_controls_sub.update(&actuator_controls)) { + _actuator_controls_throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + } + + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _flag_control_climb_rate_enabled = vehicle_control_mode.flag_control_climb_rate_enabled; + } + + if (_params.useHoverThrustEstimate) { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _params.hoverThrottle = hte.hover_thrust; + _hover_thrust_estimate_last_valid = hte.timestamp; + } + } + } + + takeoff_status_s takeoff_status; + + if (_takeoff_status_sub.update(&takeoff_status)) { + _takeoff_state = takeoff_status.takeoff_state; + } +} + +void MulticopterLandDetector::_update_params() +{ + param_get(_paramHandle.minThrottle, &_params.minThrottle); + param_get(_paramHandle.minManThrottle, &_params.minManThrottle); + param_get(_paramHandle.landSpeed, &_params.landSpeed); + + if (_param_lndmc_z_vel_max.get() > _params.landSpeed) { + PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f", + (double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed); + + _param_lndmc_z_vel_max.set(_params.landSpeed); + _param_lndmc_z_vel_max.commit_no_notification(); + } + + int32_t use_hover_thrust_estimate = 0; + param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); + _params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1); + + if (!_params.useHoverThrustEstimate || !_hover_thrust_initialized) { + param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle); + + // HTE runs based on the position controller so, even if we wish to use + // the estimate, it is only available in altitude and position modes. + // Therefore, we need to always initialize the hoverThrottle using the hover + // thrust parameter in case we fly in stabilized + // TODO: this can be removed once HTE runs in all modes + _hover_thrust_initialized = true; + } +} + +bool MulticopterLandDetector::_get_freefall_state() +{ + // norm of specific force. Should be close to 9.8 m/s^2 when landed. + return _acceleration.norm() < 2.f; +} + +bool MulticopterLandDetector::_get_ground_contact_state() +{ + const hrt_abstime time_now_us = hrt_absolute_time(); + + const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s); + + // land speed threshold, 90% of MPC_LAND_SPEED + const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f); + + if (lpos_available && _vehicle_local_position.v_z_valid) { + // Check if we are moving vertically - this might see a spike after arming due to + // throttle-up vibration. If accelerating fast the throttle thresholds will still give + // an accurate in-air indication. + float max_climb_rate = math::min(land_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get()); + + if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { + // Widen acceptance thresholds for landed state right after arming + // so that motor spool-up and other effects do not trigger false negatives. + max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f; + } + + _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate); + + } else { + _vertical_movement = true; + } + + + // Check if we are moving horizontally. + if (lpos_available && _vehicle_local_position.v_xy_valid) { + const Vector2f v_xy{_vehicle_local_position.vx, _vehicle_local_position.vy}; + _horizontal_movement = v_xy.longerThan(_param_lndmc_xy_vel_max.get()); + + } else { + _horizontal_movement = false; // not known + } + + if (lpos_available && _vehicle_local_position.dist_bottom_valid) { + _below_gnd_effect_hgt = _vehicle_local_position.dist_bottom < _get_gnd_effect_altitude(); + + } else { + _below_gnd_effect_hgt = false; + } + + const bool hover_thrust_estimate_valid = ((time_now_us - _hover_thrust_estimate_last_valid) < 1_s); + + if (!_in_descend || hover_thrust_estimate_valid) { + // continue using valid hover thrust if it became invalid during descent + _hover_thrust_estimate_valid = hover_thrust_estimate_valid; + } + + // low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available + const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f; + const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover; + _has_low_throttle = (_actuator_controls_throttle <= sys_low_throttle); + bool ground_contact = _has_low_throttle; + + // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, + // we then can assume that the vehicle hit ground + if (_flag_control_climb_rate_enabled) { + vehicle_local_position_setpoint_s trajectory_setpoint; + + if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { + // Setpoints can be NAN + _in_descend = PX4_ISFINITE(trajectory_setpoint.vz) + && (trajectory_setpoint.vz >= land_speed_threshold); + } + + // ground contact requires commanded descent until landed + if (!_maybe_landed_hysteresis.get_state() && !_landed_hysteresis.get_state()) { + ground_contact &= _in_descend; + } + + } else { + _in_descend = false; + } + + // if there is no distance to ground estimate available then don't enforce using it. + // if a distance to the ground estimate is generally available (_dist_bottom_is_observable=true), then + // we already increased the hysteresis for the land detection states in order to reduce the chance of false positives. + const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid; + _close_to_ground_or_skipped_check = _is_close_to_ground() || skip_close_to_ground_check; + + // When not armed, consider to have ground-contact + if (!_armed) { + return true; + } + + // TODO: we need an accelerometer based check for vertical movement for flying without GPS + return _close_to_ground_or_skipped_check && ground_contact && !_horizontal_movement + && !_vertical_movement; +} + +bool MulticopterLandDetector::_get_maybe_landed_state() +{ + // When not armed, consider to be maybe-landed + if (!_armed) { + return true; + } + + const hrt_abstime time_now_us = hrt_absolute_time(); + + // minimal throttle: initially 10% of throttle range between min and hover + float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f; + + // Determine the system min throttle based on flight mode + if (!_flag_control_climb_rate_enabled) { + sys_min_throttle = (_params.minManThrottle + 0.01f); + } + + // Check if thrust output is less than the minimum throttle. + if (_actuator_controls_throttle <= sys_min_throttle) { + if (_min_thrust_start == 0) { + _min_thrust_start = time_now_us; + } + + } else { + _min_thrust_start = 0; + return false; + } + + + if (_freefall_hysteresis.get_state()) { + return false; + } + + + float landThresholdFactor = 1.f; + + // Widen acceptance thresholds for landed state right after landed + if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { + landThresholdFactor = 2.5f; + } + + // Next look if all rotation angles are not moving. + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + const Vector2f angular_velocity{vehicle_angular_velocity.xyz[0], vehicle_angular_velocity.xyz[1]}; + const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor; + + if (angular_velocity.norm() > max_rotation_scaled) { + return false; + } + + // If vertical velocity is available: ground contact, no thrust, no movement -> landed + if (((time_now_us - _vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) { + return _ground_contact_hysteresis.get_state(); + } + + // Otherwise, landed if the system has minimum thrust (manual or in failsafe) and no rotation for at least 8 seconds + return (_min_thrust_start > 0) && ((time_now_us - _min_thrust_start) > 8_s); +} + +bool MulticopterLandDetector::_get_landed_state() +{ + // reset the landed_time + if (!_maybe_landed_hysteresis.get_state()) { + _landed_time = 0; + + } else if (_landed_time == 0) { + _landed_time = hrt_absolute_time(); + } + + // When not armed, consider to be landed + if (!_armed) { + return true; + } + + // if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0) + // therefore check if all other condition of the landed state remain true + return _maybe_landed_hysteresis.get_state(); +} + +float MulticopterLandDetector::_get_max_altitude() +{ + if (_param_lndmc_alt_max.get() < 0.0f) { + return INFINITY; + + } else { + return _param_lndmc_alt_max.get(); + } +} + +float MulticopterLandDetector::_get_gnd_effect_altitude() +{ + if (_param_lndmc_alt_gnd_effect.get() < 0.0f) { + return INFINITY; + + } else { + return _param_lndmc_alt_gnd_effect.get(); + } +} + +bool MulticopterLandDetector::_get_ground_effect_state() +{ + return (_in_descend && !_horizontal_movement) || + (_below_gnd_effect_hgt && _takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT) || + _takeoff_state == takeoff_status_s::TAKEOFF_STATE_RAMPUP; +} + +bool MulticopterLandDetector::_is_close_to_ground() +{ + if (_vehicle_local_position.dist_bottom_valid) { + return _vehicle_local_position.dist_bottom < DIST_FROM_GROUND_THRESHOLD; + + } else { + return false; + } +} + +void MulticopterLandDetector::_set_hysteresis_factor(const int factor) +{ + _ground_contact_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); + _landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); + _maybe_landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); + _freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US); +} + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/MulticopterLandDetector.h b/src/prometheus_px4/src/modules/land_detector/MulticopterLandDetector.h new file mode 100644 index 0000000..4045d6e --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/MulticopterLandDetector.h @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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 MulticopterLandDetector.h + * Land detection implementation for multicopters. + * + * @author Johan Jansen + * @author Morten Lysgaard + * @author Julian Oes + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "LandDetector.h" + +using namespace time_literals; + +namespace land_detector +{ + +class MulticopterLandDetector : public LandDetector +{ +public: + MulticopterLandDetector(); + ~MulticopterLandDetector() override = default; + +protected: + void _update_params() override; + void _update_topics() override; + + bool _get_landed_state() override; + bool _get_ground_contact_state() override; + bool _get_maybe_landed_state() override; + bool _get_freefall_state() override; + bool _get_ground_effect_state() override; + bool _get_in_descend() override { return _in_descend; } + bool _get_has_low_throttle() override { return _has_low_throttle; } + bool _get_horizontal_movement() override { return _horizontal_movement; } + bool _get_vertical_movement() override { return _vertical_movement; } + bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; } + float _get_max_altitude() override; + + void _set_hysteresis_factor(const int factor) override; +private: + + float _get_gnd_effect_altitude(); + bool _is_close_to_ground(); + + /** Time in us that freefall has to hold before triggering freefall */ + static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; + + /** Time interval in us in which wider acceptance thresholds are used after landed. */ + static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s; + + /** Distance above ground below which entering ground contact state is possible when distance to ground is available. */ + static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f; + + /** Handles for interesting parameters. **/ + struct { + param_t minThrottle; + param_t hoverThrottle; + param_t minManThrottle; + param_t landSpeed; + param_t useHoverThrustEstimate; + } _paramHandle{}; + + struct { + float minThrottle; + float hoverThrottle; + float minManThrottle; + float landSpeed; + bool useHoverThrustEstimate; + } _params{}; + + uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + + hrt_abstime _hover_thrust_estimate_last_valid{0}; + bool _hover_thrust_estimate_valid{false}; + + bool _flag_control_climb_rate_enabled{false}; + bool _hover_thrust_initialized{false}; + + float _actuator_controls_throttle{0.f}; + + uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED}; + + hrt_abstime _min_thrust_start{0}; ///< timestamp when minimum trust was applied first + hrt_abstime _landed_time{0}; + + bool _in_descend{false}; ///< vehicle is desending + bool _horizontal_movement{false}; ///< vehicle is moving horizontally + bool _vertical_movement{false}; + bool _has_low_throttle{false}; + bool _close_to_ground_or_skipped_check{false}; + bool _below_gnd_effect_hgt{false}; ///< vehicle height above ground is below height where ground effect occurs + + DEFINE_PARAMETERS_CUSTOM_PARENT( + LandDetector, + (ParamFloat) _param_lndmc_trig_time, + (ParamFloat) _param_lndmc_alt_max, + (ParamFloat) _param_lndmc_rot_max, + (ParamFloat) _param_lndmc_xy_vel_max, + (ParamFloat) _param_lndmc_z_vel_max, + (ParamFloat) _param_lndmc_alt_gnd_effect + ); +}; + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/RoverLandDetector.cpp b/src/prometheus_px4/src/modules/land_detector/RoverLandDetector.cpp new file mode 100644 index 0000000..e9942c4 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/RoverLandDetector.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 RoverLandDetector.cpp + * Land detection algorithm for Rovers + * + * @author Roman Bapst + * @author Julian Oes + */ + +#include "RoverLandDetector.h" + +namespace land_detector +{ + +bool RoverLandDetector::_get_ground_contact_state() +{ + return true; +} + +bool RoverLandDetector::_get_landed_state() +{ + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { + return true; // If Landing has been requested then say we have landed. + + } else { + return !_armed; // If we are armed we are not landed. + } +} + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/RoverLandDetector.h b/src/prometheus_px4/src/modules/land_detector/RoverLandDetector.h new file mode 100644 index 0000000..5ebc5b6 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/RoverLandDetector.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 RoverLandDetector.h + * Land detection implementation for VTOL also called hybrids. + * + * @author Roman Bapst + * @author Julian Oes + */ + +#pragma once + +#include "LandDetector.h" + +namespace land_detector +{ + +class RoverLandDetector : public LandDetector +{ +public: + RoverLandDetector() = default; + ~RoverLandDetector() override = default; + +protected: + bool _get_ground_contact_state() override; + bool _get_landed_state() override; + void _set_hysteresis_factor(const int factor) override {}; +}; + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/VtolLandDetector.cpp b/src/prometheus_px4/src/modules/land_detector/VtolLandDetector.cpp new file mode 100644 index 0000000..a3a4ae3 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/VtolLandDetector.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 VtolLandDetector.cpp + * Land detection algorithm for VTOL + * + * @author Roman Bapst + * @author Julian Oes + */ + +#include +#include + +#include "VtolLandDetector.h" + +namespace land_detector +{ + +void VtolLandDetector::_update_topics() +{ + MulticopterLandDetector::_update_topics(); +} + +bool VtolLandDetector::_get_maybe_landed_state() +{ + // If in Fixed-wing mode, only trigger if disarmed + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return !_armed; + } + + return MulticopterLandDetector::_get_maybe_landed_state(); +} + +bool VtolLandDetector::_get_landed_state() +{ + // If in Fixed-wing mode, only trigger if disarmed + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return !_armed; + } + + // this is returned from the mutlicopter land detector + bool landed = MulticopterLandDetector::_get_landed_state(); + + // for vtol we additionally consider airspeed + airspeed_validated_s airspeed_validated{}; + _airspeed_validated_sub.copy(&airspeed_validated); + + if (hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) { + + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s; + + } else { + // if airspeed does not update, set it to zero and rely on multicopter land detector + _airspeed_filtered = 0.0f; + } + + // only consider airspeed if we have been in air before to avoid false + // detections in the case of wind on the ground + if (_was_in_air && (_airspeed_filtered > _param_lndfw_airspd_max.get())) { + landed = false; + } + + _was_in_air = !landed; + + return landed; +} + +bool VtolLandDetector::_get_freefall_state() +{ + // true if falling or in a parabolic flight (low gravity) + bool free_fall_detected = MulticopterLandDetector::_get_freefall_state(); + + // only return a positive free fall detected if not in fixed-wing mode + return _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && free_fall_detected; +} + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/VtolLandDetector.h b/src/prometheus_px4/src/modules/land_detector/VtolLandDetector.h new file mode 100644 index 0000000..16b34b6 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/VtolLandDetector.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 VtolLandDetector.h + * Land detection implementation for VTOL also called hybrids. + * + * @author Roman Bapst + * @author Julian Oes + */ + +#pragma once + +#include + +#include "MulticopterLandDetector.h" + +namespace land_detector +{ + +class VtolLandDetector : public MulticopterLandDetector +{ +public: + VtolLandDetector() = default; + ~VtolLandDetector() override = default; + +protected: + void _update_topics() override; + bool _get_landed_state() override; + bool _get_maybe_landed_state() override; + bool _get_freefall_state() override; + +private: + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + + bool _was_in_air{false}; /**< indicates whether the vehicle was in the air in the previous iteration */ + float _airspeed_filtered{0.0f}; /**< low pass filtered airspeed */ + + DEFINE_PARAMETERS_CUSTOM_PARENT( + MulticopterLandDetector, + (ParamFloat) _param_lndfw_airspd_max + ); +}; + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/land_detector_main.cpp b/src/prometheus_px4/src/modules/land_detector/land_detector_main.cpp new file mode 100644 index 0000000..a64bdc9 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/land_detector_main.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 land_detector_main.cpp + * Land detection algorithm + * + * @author Johan Jansen + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include + +#include "FixedwingLandDetector.h" +#include "MulticopterLandDetector.h" +#include "RoverLandDetector.h" +#include "VtolLandDetector.h" +#include "AirshipLandDetector.h" + + +namespace land_detector +{ + +static char _currentMode[12]; + +int LandDetector::task_spawn(int argc, char *argv[]) +{ + if (argc < 2) { + print_usage(); + return PX4_ERROR; + } + + LandDetector *obj = nullptr; + + if (strcmp(argv[1], "fixedwing") == 0) { + obj = new FixedwingLandDetector(); + + } else if (strcmp(argv[1], "multicopter") == 0) { + obj = new MulticopterLandDetector(); + + } else if (strcmp(argv[1], "vtol") == 0) { + obj = new VtolLandDetector(); + + } else if (strcmp(argv[1], "rover") == 0) { + obj = new RoverLandDetector(); + + } else if (strcmp(argv[1], "airship") == 0) { + obj = new AirshipLandDetector(); + + } else { + print_usage("unknown mode"); + return PX4_ERROR; + } + + if (obj == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + // Remember current active mode + strncpy(_currentMode, argv[1], sizeof(_currentMode) - 1); + _currentMode[sizeof(_currentMode) - 1] = '\0'; + + _object.store(obj); + _task_id = task_id_is_work_queue; + + obj->start(); + + return PX4_OK; +} + +int LandDetector::print_status() +{ + PX4_INFO("running (%s)", _currentMode); + return 0; +} +int LandDetector::print_usage(const char *reason) +{ + if (reason != nullptr) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Module to detect the freefall and landed state of the vehicle, and publishing the `vehicle_land_detected` topic. +Each vehicle type (multirotor, fixedwing, vtol, ...) provides its own algorithm, taking into account various +states, such as commanded thrust, arming state and vehicle motion. + +### Implementation +Every type is implemented in its own class with a common base class. The base class maintains a state (landed, +maybe_landed, ground_contact). Each possible state is implemented in the derived classes. A hysteresis and a fixed +priority of each internal state determines the actual land_detector state. + +#### Multicopter Land Detector +**ground_contact**: thrust setpoint and velocity in z-direction must be below a defined threshold for time +GROUND_CONTACT_TRIGGER_TIME_US. When ground_contact is detected, the position controller turns off the thrust setpoint +in body x and y. + +**maybe_landed**: it requires ground_contact together with a tighter thrust setpoint threshold and no velocity in the +horizontal direction. The trigger time is defined by MAYBE_LAND_TRIGGER_TIME. When maybe_landed is detected, the +position controller sets the thrust setpoint to zero. + +**landed**: it requires maybe_landed to be true for time LAND_DETECTOR_TRIGGER_TIME_US. + +The module runs periodically on the HP work queue. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("land_detector", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover|airship", "Select vehicle type", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int land_detector_main(int argc, char *argv[]) +{ + return LandDetector::main(argc, argv); +} + +} // namespace land_detector diff --git a/src/prometheus_px4/src/modules/land_detector/land_detector_params.c b/src/prometheus_px4/src/modules/land_detector/land_detector_params.c new file mode 100644 index 0000000..950956a --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/land_detector_params.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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. + * + ****************************************************************************/ + +/** + * Total flight time in microseconds + * + * Total flight time of this autopilot. Higher 32 bits of the value. + * Flight time in microseconds = (LND_FLIGHT_T_HI << 32) | LND_FLIGHT_T_LO. + * + * @min 0 + * @volatile + * @category system + * @group Land Detector + * + */ +PARAM_DEFINE_INT32(LND_FLIGHT_T_HI, 0); + +/** + * Total flight time in microseconds + * + * Total flight time of this autopilot. Lower 32 bits of the value. + * Flight time in microseconds = (LND_FLIGHT_T_HI << 32) | LND_FLIGHT_T_LO. + * + * @min 0 + * @volatile + * @category system + * @group Land Detector + * + */ +PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0); diff --git a/src/prometheus_px4/src/modules/land_detector/land_detector_params_fw.c b/src/prometheus_px4/src/modules/land_detector/land_detector_params_fw.c new file mode 100644 index 0000000..b33c670 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/land_detector_params_fw.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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. + * + ****************************************************************************/ + +/** + * Fixedwing max horizontal velocity + * + * Maximum horizontal velocity allowed in the landed state + * + * @unit m/s + * @min 0.5 + * @max 10 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); + +/** + * Fixedwing max climb rate + * + * Maximum vertical velocity allowed in the landed state + * + * @unit m/s + * @min 0.1 + * @max 20 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 3.0f); + +/** + * Fixedwing max horizontal acceleration + * + * Maximum horizontal (x,y body axes) acceleration allowed in the landed state + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_XYACC_MAX, 8.0f); + +/** + * Airspeed max + * + * Maximum airspeed allowed in the landed state + * + * @unit m/s + * @min 4 + * @max 20 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 6.00f); diff --git a/src/prometheus_px4/src/modules/land_detector/land_detector_params_mc.c b/src/prometheus_px4/src/modules/land_detector/land_detector_params_mc.c new file mode 100644 index 0000000..cdfe6d5 --- /dev/null +++ b/src/prometheus_px4/src/modules/land_detector/land_detector_params_mc.c @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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. + * + ****************************************************************************/ + +/** + * Multicopter land detection trigger time + * + * Total time it takes to go through all three land detection stages: + * ground contact, maybe landed, landed + * when all necessary conditions are constantly met. + * + * @unit s + * @min 0.1 + * @max 10.0 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); + +/** + * Multicopter max climb rate + * + * Maximum vertical velocity allowed in the landed state + * + * @unit m/s + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f); + +/** + * Multicopter max horizontal velocity + * + * Maximum horizontal velocity allowed in the landed state + * + * @unit m/s + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); + +/** + * Multicopter max rotation + * + * Maximum allowed angular velocity around each axis allowed in the landed state. + * + * @unit deg/s + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); + +/** + * Maximum altitude for multicopters + * + * The system will obey this limit as a + * hard altitude limit. This setting will + * be consolidated with the GF_MAX_VER_DIST + * parameter. + * A negative value indicates no altitude limitation. + * + * @unit m + * @min -1 + * @max 10000 + * @decimal 2 + * @group Land Detector + * + */ +PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f); + +/** + * Ground effect altitude for multicopters + * + * The height above ground below which ground effect creates barometric altitude errors. + * A negative value indicates no ground effect. + * + * @unit m + * @min -1 + * @decimal 2 + * @group Land Detector + * + */ +PARAM_DEFINE_FLOAT(LNDMC_ALT_GND, -1.0f); diff --git a/src/prometheus_px4/src/modules/landing_target_estimator/CMakeLists.txt b/src/prometheus_px4/src/modules/landing_target_estimator/CMakeLists.txt new file mode 100644 index 0000000..146869a --- /dev/null +++ b/src/prometheus_px4/src/modules/landing_target_estimator/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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__landing_target_estimator + MAIN landing_target_estimator + COMPILE_FLAGS + SRCS + landing_target_estimator_main.cpp + LandingTargetEstimator.cpp + KalmanFilter.cpp + DEPENDS + ) + diff --git a/src/prometheus_px4/src/modules/landing_target_estimator/KalmanFilter.cpp b/src/prometheus_px4/src/modules/landing_target_estimator/KalmanFilter.cpp new file mode 100644 index 0000000..e4636b6 --- /dev/null +++ b/src/prometheus_px4/src/modules/landing_target_estimator/KalmanFilter.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/* + * @file KalmanFilter.h + * Simple Kalman Filter for variable gain low-passing + * + * @author Nicolas de Palezieux (Sunflower Labs) + * + */ + +#include "KalmanFilter.h" + +namespace landing_target_estimator +{ +KalmanFilter::KalmanFilter(matrix::Vector &initial, matrix::Matrix &covInit) +{ + init(initial, covInit); +} + +void KalmanFilter::init(matrix::Vector &initial, matrix::Matrix &covInit) +{ + _x = initial; + _covariance = covInit; +} + +void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11) +{ + matrix::Vector initial; + initial(0) = initial0; + initial(1) = initial1; + matrix::Matrix covInit; + covInit(0, 0) = covInit00; + covInit(1, 1) = covInit11; + + init(initial, covInit); +} + +void KalmanFilter::predict(float dt, float acc, float acc_unc) +{ + _x(0) += _x(1) * dt + dt * dt / 2 * acc; + _x(1) += acc * dt; + + matrix::Matrix A; // propagation matrix + A(0, 0) = 1; + A(1, 1) = 1; + A(0, 1) = dt; + + matrix::Matrix G; // noise model + G(0, 0) = dt * dt / 2; + G(1, 0) = dt; + + matrix::Matrix process_noise = G * G.transpose() * acc_unc; + + _covariance = A * _covariance * A.transpose() + process_noise; +} + +bool KalmanFilter::update(float meas, float measUnc) +{ + + // H = [1, 0] + _residual = meas - _x(0); + + // H * P * H^T simply selects P(0,0) + _innovCov = _covariance(0, 0) + measUnc; + + // outlier rejection + float beta = _residual / _innovCov * _residual; + + // 5% false alarm probability + if (beta > 3.84f) { + return false; + } + + matrix::Vector kalmanGain; + kalmanGain(0) = _covariance(0, 0); + kalmanGain(1) = _covariance(1, 0); + kalmanGain /= _innovCov; + + _x += kalmanGain * _residual; + + matrix::Matrix identity; + identity.identity(); + + matrix::Matrix KH; // kalmanGain * H + KH(0, 0) = kalmanGain(0); + KH(1, 0) = kalmanGain(1); + + _covariance = (identity - KH) * _covariance; + + return true; + +} + +void KalmanFilter::getState(matrix::Vector &state) +{ + state = _x; +} + +void KalmanFilter::getState(float &state0, float &state1) +{ + state0 = _x(0); + state1 = _x(1); +} + +void KalmanFilter::getCovariance(matrix::Matrix &covariance) +{ + covariance = _covariance; +} + +void KalmanFilter::getCovariance(float &cov00, float &cov11) +{ + cov00 = _covariance(0, 0); + cov11 = _covariance(1, 1); +} + +void KalmanFilter::getInnovations(float &innov, float &innovCov) +{ + innov = _residual; + innovCov = _innovCov; +} + +} // namespace landing_target_estimator diff --git a/src/prometheus_px4/src/modules/landing_target_estimator/KalmanFilter.h b/src/prometheus_px4/src/modules/landing_target_estimator/KalmanFilter.h new file mode 100644 index 0000000..3d466d6 --- /dev/null +++ b/src/prometheus_px4/src/modules/landing_target_estimator/KalmanFilter.h @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/* + * @file KalmanFilter.h + * Simple Kalman Filter for variable gain low-passing + * + * Constant velocity model. Prediction according to + * x_{k+1} = A * x_{k} + * with A = [1 dt; 0 1] + * + * Update with a direct measurement of the first state: + * H = [1 0] + * + * @author Nicolas de Palezieux (Sunflower Labs) + * + */ + +#include +#include +#include +#include + +#pragma once + +namespace landing_target_estimator +{ +class KalmanFilter +{ +public: + /** + * Default constructor, state not initialized + */ + KalmanFilter() {}; + + /** + * Constructor, initialize state + */ + KalmanFilter(matrix::Vector &initial, matrix::Matrix &covInit); + + /** + * Default desctructor + */ + virtual ~KalmanFilter() {}; + + /** + * Initialize filter state + * @param initial initial state + * @param covInit initial covariance + */ + void init(matrix::Vector &initial, matrix::Matrix &covInit); + + /** + * Initialize filter state, only specifying diagonal covariance elements + * @param initial0 first initial state + * @param initial1 second initial state + * @param covInit00 initial variance of first state + * @param covinit11 initial variance of second state + */ + void init(float initial0, float initial1, float covInit00, float covInit11); + + /** + * Predict the state with an external acceleration estimate + * @param dt Time delta in seconds since last state change + * @param acc Acceleration estimate + * @param acc_unc Variance of acceleration estimate + */ + void predict(float dt, float acc, float acc_unc); + + /** + * Update the state estimate with a measurement + * @param meas state measeasurement + * @param measUnc measurement uncertainty + * @return update success (measurement not rejected) + */ + bool update(float meas, float measUnc); + + /** + * Get the current filter state + * @param x1 State + */ + void getState(matrix::Vector &state); + + /** + * Get the current filter state + * @param state0 First state + * @param state1 Second state + */ + void getState(float &state0, float &state1); + + /** + * Get state covariance + * @param covariance Covariance of the state + */ + void getCovariance(matrix::Matrix &covariance); + + /** + * Get state variances (diagonal elements) + * @param cov00 Variance of first state + * @param cov11 Variance of second state + */ + void getCovariance(float &cov00, float &cov11); + + /** + * Get measurement innovation and covariance of last update call + * @param innov Measurement innovation + * @param innovCov Measurement innovation covariance + */ + void getInnovations(float &innov, float &innovCov); + +private: + matrix::Vector _x; // state + + matrix::Matrix _covariance; // state covariance + + float _residual{0.0f}; // residual of last measurement update + + float _innovCov{0.0f}; // innovation covariance of last measurement update +}; +} // namespace landing_target_estimator diff --git a/src/prometheus_px4/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/prometheus_px4/src/modules/landing_target_estimator/LandingTargetEstimator.cpp new file mode 100644 index 0000000..bc0249a --- /dev/null +++ b/src/prometheus_px4/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/* + * @file LandingTargetEstimator.cpp + * + * @author Nicolas de Palezieux (Sunflower Labs) + * @author Mohammed Kabir + * + */ + +#include +#include +#include + +#include "LandingTargetEstimator.h" + +#define SEC2USEC 1000000.0f + + +namespace landing_target_estimator +{ + +LandingTargetEstimator::LandingTargetEstimator() +{ + _paramHandle.acc_unc = param_find("LTEST_ACC_UNC"); + _paramHandle.meas_unc = param_find("LTEST_MEAS_UNC"); + _paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN"); + _paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN"); + _paramHandle.mode = param_find("LTEST_MODE"); + _paramHandle.scale_x = param_find("LTEST_SCALE_X"); + _paramHandle.scale_y = param_find("LTEST_SCALE_Y"); + + _check_params(true); +} + +void LandingTargetEstimator::update() +{ + _check_params(false); + + _update_topics(); + + /* predict */ + if (_estimator_initialized) { + if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) { + PX4_WARN("Timeout"); + _estimator_initialized = false; + + } else { + float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC; + + // predict target position with the help of accel data + matrix::Vector3f a{_vehicle_acceleration.xyz}; + + if (_vehicleAttitude_valid && _vehicle_acceleration_valid) { + matrix::Quaternion q_att(&_vehicleAttitude.q[0]); + _R_att = matrix::Dcm(q_att); + a = _R_att * a; + + } else { + a.zero(); + } + + _kalman_filter_x.predict(dt, -a(0), _params.acc_unc); + _kalman_filter_y.predict(dt, -a(1), _params.acc_unc); + + _last_predict = hrt_absolute_time(); + } + } + + if (!_new_irlockReport) { + // nothing to do + return; + } + + // mark this sensor measurement as consumed + _new_irlockReport = false; + + if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) { + // don't have the data needed for an update + return; + } + + if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) { + return; + } + + // TODO account for sensor orientation as set by parameter + // default orientation has camera x pointing in body y, camera y in body -x + + matrix::Vector sensor_ray; // ray pointing towards target in body frame + sensor_ray(0) = -_irlockReport.pos_y * _params.scale_y; // forward + sensor_ray(1) = _irlockReport.pos_x * _params.scale_x; // right + sensor_ray(2) = 1.0f; + + // rotate the unit ray into the navigation frame, assume sensor frame = body frame + matrix::Quaternion q_att(&_vehicleAttitude.q[0]); + _R_att = matrix::Dcm(q_att); + sensor_ray = _R_att * sensor_ray; + + if (fabsf(sensor_ray(2)) < 1e-6f) { + // z component of measurement unsafe, don't use this measurement + return; + } + + float dist = _vehicleLocalPosition.dist_bottom; + + // scale the ray s.t. the z component has length of dist + _rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist; + _rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist; + + if (!_estimator_initialized) { + PX4_INFO("Init"); + float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f; + float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f; + _kalman_filter_x.init(_rel_pos(0), vx_init, _params.pos_unc_init, _params.vel_unc_init); + _kalman_filter_y.init(_rel_pos(1), vy_init, _params.pos_unc_init, _params.vel_unc_init); + + _estimator_initialized = true; + _last_update = hrt_absolute_time(); + _last_predict = _last_update; + + } else { + // update + bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist); + bool update_y = _kalman_filter_y.update(_rel_pos(1), _params.meas_unc * dist * dist); + + if (!update_x || !update_y) { + if (!_faulty) { + _faulty = true; + PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y"); + } + + } else { + _faulty = false; + } + + if (!_faulty) { + // only publish if both measurements were good + + _target_pose.timestamp = _irlockReport.timestamp; + + float x, xvel, y, yvel, covx, covx_v, covy, covy_v; + _kalman_filter_x.getState(x, xvel); + _kalman_filter_x.getCovariance(covx, covx_v); + + _kalman_filter_y.getState(y, yvel); + _kalman_filter_y.getCovariance(covy, covy_v); + + _target_pose.is_static = (_params.mode == TargetMode::Stationary); + + _target_pose.rel_pos_valid = true; + _target_pose.rel_vel_valid = true; + _target_pose.x_rel = x; + _target_pose.y_rel = y; + _target_pose.z_rel = dist; + _target_pose.vx_rel = xvel; + _target_pose.vy_rel = yvel; + + _target_pose.cov_x_rel = covx; + _target_pose.cov_y_rel = covy; + + _target_pose.cov_vx_rel = covx_v; + _target_pose.cov_vy_rel = covy_v; + + if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) { + _target_pose.x_abs = x + _vehicleLocalPosition.x; + _target_pose.y_abs = y + _vehicleLocalPosition.y; + _target_pose.z_abs = dist + _vehicleLocalPosition.z; + _target_pose.abs_pos_valid = true; + + } else { + _target_pose.abs_pos_valid = false; + } + + _targetPosePub.publish(_target_pose); + + _last_update = hrt_absolute_time(); + _last_predict = _last_update; + } + + float innov_x, innov_cov_x, innov_y, innov_cov_y; + _kalman_filter_x.getInnovations(innov_x, innov_cov_x); + _kalman_filter_y.getInnovations(innov_y, innov_cov_y); + + _target_innovations.timestamp = _irlockReport.timestamp; + _target_innovations.innov_x = innov_x; + _target_innovations.innov_cov_x = innov_cov_x; + _target_innovations.innov_y = innov_y; + _target_innovations.innov_cov_y = innov_cov_y; + + _targetInnovationsPub.publish(_target_innovations); + } +} + +void LandingTargetEstimator::_check_params(const bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + _update_params(); + } +} + +void LandingTargetEstimator::_update_topics() +{ + _vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition); + _vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude); + _vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration); + + _new_irlockReport = _irlockReportSub.update(&_irlockReport); +} + +void LandingTargetEstimator::_update_params() +{ + param_get(_paramHandle.acc_unc, &_params.acc_unc); + param_get(_paramHandle.meas_unc, &_params.meas_unc); + param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init); + param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init); + + int32_t mode = 0; + param_get(_paramHandle.mode, &mode); + _params.mode = (TargetMode)mode; + + param_get(_paramHandle.scale_x, &_params.scale_x); + param_get(_paramHandle.scale_y, &_params.scale_y); +} + + +} // namespace landing_target_estimator diff --git a/src/prometheus_px4/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/prometheus_px4/src/modules/landing_target_estimator/LandingTargetEstimator.h new file mode 100644 index 0000000..55a2d47 --- /dev/null +++ b/src/prometheus_px4/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/* + * @file LandingTargetEstimator.h + * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor. + * + * @author Nicolas de Palezieux (Sunflower Labs) + * @author Mohammed Kabir + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "KalmanFilter.h" + +using namespace time_literals; + +namespace landing_target_estimator +{ + +class LandingTargetEstimator +{ +public: + + LandingTargetEstimator(); + virtual ~LandingTargetEstimator() = default; + + /* + * Get new measurements and update the state estimate + */ + void update(); + +protected: + + /* + * Update uORB topics. + */ + void _update_topics(); + + /* + * Update parameters. + */ + void _update_params(); + + /* timeout after which filter is reset if target not seen */ + static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000; + + uORB::Publication _targetPosePub{ORB_ID(landing_target_pose)}; + landing_target_pose_s _target_pose{}; + + uORB::Publication _targetInnovationsPub{ORB_ID(landing_target_innovations)}; + landing_target_innovations_s _target_innovations{}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + +private: + + enum class TargetMode { + Moving = 0, + Stationary + }; + + /** + * Handles for parameters + **/ + struct { + param_t acc_unc; + param_t meas_unc; + param_t pos_unc_init; + param_t vel_unc_init; + param_t mode; + param_t scale_x; + param_t scale_y; + } _paramHandle; + + struct { + float acc_unc; + float meas_unc; + float pos_unc_init; + float vel_unc_init; + TargetMode mode; + float scale_x; + float scale_y; + } _params; + + uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)}; + + vehicle_local_position_s _vehicleLocalPosition{}; + vehicle_attitude_s _vehicleAttitude{}; + vehicle_acceleration_s _vehicle_acceleration{}; + irlock_report_s _irlockReport{}; + + // keep track of which topics we have received + bool _vehicleLocalPosition_valid{false}; + bool _vehicleAttitude_valid{false}; + bool _vehicle_acceleration_valid{false}; + bool _new_irlockReport{false}; + bool _estimator_initialized{false}; + // keep track of whether last measurement was rejected + bool _faulty{false}; + + matrix::Dcmf _R_att; + matrix::Vector2f _rel_pos; + KalmanFilter _kalman_filter_x; + KalmanFilter _kalman_filter_y; + hrt_abstime _last_predict{0}; // timestamp of last filter prediction + hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout) + + void _check_params(const bool force); + + void _update_state(); +}; + + +} // namespace landing_target_estimator diff --git a/src/prometheus_px4/src/modules/landing_target_estimator/landing_target_estimator_main.cpp b/src/prometheus_px4/src/modules/landing_target_estimator/landing_target_estimator_main.cpp new file mode 100644 index 0000000..5cbe7ee --- /dev/null +++ b/src/prometheus_px4/src/modules/landing_target_estimator/landing_target_estimator_main.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file landing_target_estimator_main.cpp + * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor. + * + * @author Nicolas de Palezieux (Sunflower Labs) + * @author Mohammed Kabir + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "LandingTargetEstimator.h" + + +namespace landing_target_estimator +{ + +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* Run main loop at this rate in Hz. */ +static constexpr uint32_t landing_target_estimator_UPDATE_RATE_HZ = 50; + +/** + * Landing target position estimator app start / stop handling function + * This makes the module accessible from the nuttx shell + * @ingroup apps + */ +extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int landing_target_estimator_thread_main(int argc, char *argv[]); + +/** +* Main entry point for this module +**/ +int landing_target_estimator_main(int argc, char *argv[]) +{ + + if (argc < 2) { + goto exiterr; + } + + if (argc >= 2 && !strcmp(argv[1], "start")) { + if (thread_running) { + PX4_INFO("already running"); + /* this is not an error */ + return 0; + } + + thread_should_exit = false; + daemon_task = px4_task_spawn_cmd("landing_target_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + landing_target_estimator_thread_main, + (argv) ? (char *const *)&argv[2] : nullptr); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + + if (!thread_running) { + PX4_WARN("landing_target_estimator not running"); + } + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + PX4_INFO("running"); + + } else { + PX4_INFO("not started"); + } + + return 0; + } + +exiterr: + PX4_WARN("usage: landing_target_estimator {start|stop|status}"); + return 1; +} + +int landing_target_estimator_thread_main(int argc, char *argv[]) +{ + PX4_DEBUG("starting"); + + thread_running = true; + + LandingTargetEstimator est; + + while (!thread_should_exit) { + est.update(); + px4_usleep(1000000 / landing_target_estimator_UPDATE_RATE_HZ); + } + + PX4_DEBUG("exiting"); + + thread_running = false; + + return 0; +} + +} diff --git a/src/prometheus_px4/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/prometheus_px4/src/modules/landing_target_estimator/landing_target_estimator_params.c new file mode 100644 index 0000000..252fc2f --- /dev/null +++ b/src/prometheus_px4/src/modules/landing_target_estimator/landing_target_estimator_params.c @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 landing_target_estimator_params.c + * Landing target estimator algorithm parameters. + * + * @author Nicolas de Palezieux (Sunflower Labs) + * @author Mohammed Kabir + * + */ + +/** + * Landing target mode + * + * Configure the mode of the landing target. Depending on the mode, the landing target observations are used differently to aid position estimation. + * + * Mode Moving: The landing target may be moving around while in the field of view of the vehicle. Landing target measurements are not used to aid positioning. + * Mode Stationary: The landing target is stationary. Measured velocity w.r.t. the landing target is used to aid velocity estimation. + * + * @min 0 + * @max 1 + * @group Landing target Estimator + * @value 0 Moving + * @value 1 Stationary + */ +PARAM_DEFINE_INT32(LTEST_MODE, 0); + +/** + * Acceleration uncertainty + * + * Variance of acceleration measurement used for landing target position prediction. + * Higher values results in tighter following of the measurements and more lenient outlier rejection + * + * @unit (m/s^2)^2 + * @min 0.01 + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_ACC_UNC, 10.0f); + +/** + * Landing target measurement uncertainty + * + * Variance of the landing target measurement from the driver. + * Higher values results in less agressive following of the measurement and a smoother output as well as fewer rejected measurements. + * + * @unit tan(rad)^2 + * @decimal 4 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_MEAS_UNC, 0.005f); + +/** + * Initial landing target position uncertainty + * + * Initial variance of the relative landing target position in x and y direction + * + * @unit m^2 + * @min 0.001 + * @decimal 3 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f); + +/** + * Initial landing target velocity uncertainty + * + * Initial variance of the relative landing target velocity in x and y direction + * + * @unit (m/s)^2 + * @min 0.001 + * @decimal 3 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 0.1f); + +/** + * Scale factor for sensor measurements in sensor x axis + * + * Landing target x measurements are scaled by this factor before being used + * + * @min 0.01 + * @decimal 3 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f); + +/** + * Scale factor for sensor measurements in sensor y axis + * + * Landing target y measurements are scaled by this factor before being used + * + * @min 0.01 + * @decimal 3 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f); diff --git a/src/prometheus_px4/src/modules/load_mon/CMakeLists.txt b/src/prometheus_px4/src/modules/load_mon/CMakeLists.txt new file mode 100644 index 0000000..a2c6ee0 --- /dev/null +++ b/src/prometheus_px4/src/modules/load_mon/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2016-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__load_mon + MAIN load_mon + COMPILE_FLAGS + SRCS + LoadMon.cpp + LoadMon.hpp + DEPENDS + px4_work_queue +) diff --git a/src/prometheus_px4/src/modules/load_mon/LoadMon.cpp b/src/prometheus_px4/src/modules/load_mon/LoadMon.cpp new file mode 100644 index 0000000..ea22b60 --- /dev/null +++ b/src/prometheus_px4/src/modules/load_mon/LoadMon.cpp @@ -0,0 +1,333 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 "LoadMon.hpp" + +#if defined(__PX4_NUTTX) +// if free stack space falls below this, print a warning +#if defined(CONFIG_ARMV7M_STACKCHECK) +static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 100; +#else +static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 300; +#endif + +static constexpr unsigned FDS_LOW_WARNING_THRESHOLD = 2; ///< if free file descriptors fall below this, print a warning +#endif + +using namespace time_literals; + +namespace load_mon +{ + +LoadMon::LoadMon() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ +} + +LoadMon::~LoadMon() +{ + ScheduleClear(); + perf_free(_cycle_perf); +} + +int LoadMon::task_spawn(int argc, char *argv[]) +{ + LoadMon *obj = new LoadMon(); + + if (!obj) { + PX4_ERR("alloc failed"); + return -1; + } + + _object.store(obj); + _task_id = task_id_is_work_queue; + + /* Schedule a cycle to start things. */ + obj->start(); + + return 0; +} + +void LoadMon::start() +{ + ScheduleOnInterval(500_ms); // 2 Hz +} + +void LoadMon::Run() +{ +#if defined (__PX4_LINUX) + + if (_proc_fd == nullptr) { // init fd + _proc_fd = fopen("/proc/meminfo", "r"); + + if (_proc_fd == nullptr) { + PX4_ERR("Failed to open /proc/meminfo"); + } + } + +#endif + perf_begin(_cycle_perf); + + cpuload(); + +#if defined(__PX4_NUTTX) + + if (_param_sys_stck_en.get()) { + stack_usage(); + } + +#endif + + if (should_exit()) { + ScheduleClear(); +#if defined (__PX4_LINUX) + fclose(_proc_fd); +#endif + exit_and_cleanup(); + } + + perf_end(_cycle_perf); +} + +void LoadMon::cpuload() +{ +#if defined(__PX4_LINUX) + tms spent_time_stamp_struct; + clock_t total_time_stamp = times(&spent_time_stamp_struct); + clock_t spent_time_stamp = spent_time_stamp_struct.tms_utime + spent_time_stamp_struct.tms_stime; + + if (_last_total_time_stamp == 0 || _last_spent_time_stamp == 0) { + // Just get the time in the first iteration */ + _last_total_time_stamp = total_time_stamp; + _last_spent_time_stamp = spent_time_stamp; + return; + } + + // compute system load + const float interval = total_time_stamp - _last_total_time_stamp; + const float interval_spent_time = spent_time_stamp - _last_spent_time_stamp; +#elif defined(__PX4_NUTTX) + + if (_last_idle_time == 0) { + // Just get the time in the first iteration */ + _last_idle_time = system_load.tasks[0].total_runtime; + _last_idle_time_sample = hrt_absolute_time(); + return; + } + + irqstate_t irqstate = enter_critical_section(); + const hrt_abstime now = hrt_absolute_time(); + const hrt_abstime total_runtime = system_load.tasks[0].total_runtime; + leave_critical_section(irqstate); + + // compute system load + const float interval = now - _last_idle_time_sample; + const float interval_idletime = total_runtime - _last_idle_time; +#endif + + cpuload_s cpuload{}; +#if defined(__PX4_LINUX) + /* following calculation is based on free(1) + * https://gitlab.com/procps-ng/procps/-/blob/master/proc/sysinfo.c */ + char line[256]; + int32_t kb_main_total = -1; + int32_t kb_main_free = -1; + int32_t kb_page_cache = -1; + int32_t kb_slab_reclaimable = -1; + int32_t kb_main_buffers = -1; + int parsedCount = 0; + + if (_proc_fd != nullptr) { + while (fgets(line, sizeof(line), _proc_fd)) { + if (sscanf(line, "MemTotal: %d kB", &kb_main_total) == 1) { + ++parsedCount; + continue; + } + + if (sscanf(line, "MemFree: %d kB", &kb_main_free) == 1) { + ++parsedCount; + continue; + } + + if (sscanf(line, "Cached: %d kB", &kb_page_cache) == 1) { + ++parsedCount; + continue; + } + + if (sscanf(line, "SReclaimable: %d kB", &kb_slab_reclaimable) == 1) { + ++parsedCount; + continue; + } + + if (sscanf(line, "Buffers: %d kB", &kb_main_buffers) == 1) { + ++parsedCount; + continue; + } + } + + fseek(_proc_fd, 0, SEEK_END); + + if (parsedCount == 5) { + int32_t kb_main_cached = kb_page_cache + kb_slab_reclaimable; + int32_t mem_used = kb_main_total - kb_main_free - kb_main_cached - kb_main_buffers; + + if (mem_used < 0) { + mem_used = kb_main_total - kb_main_free; + } + + cpuload.ram_usage = (float)mem_used / kb_main_total; + + } else { + PX4_ERR("Could not parse /proc/meminfo"); + cpuload.ram_usage = -1; + } + + } else { + cpuload.ram_usage = -1; + } + + cpuload.load = interval_spent_time / interval; +#elif defined(__PX4_NUTTX) + // get ram usage + struct mallinfo mem = mallinfo(); + cpuload.ram_usage = (float)mem.uordblks / mem.arena; + cpuload.load = 1.f - interval_idletime / interval; +#endif + cpuload.timestamp = hrt_absolute_time(); + + _cpuload_pub.publish(cpuload); + + // store for next iteration +#if defined(__PX4_LINUX) + _last_total_time_stamp = total_time_stamp; + _last_spent_time_stamp = spent_time_stamp; +#elif defined(__PX4_NUTTX) + _last_idle_time = total_runtime; + _last_idle_time_sample = now; +#endif +} + +#if defined(__PX4_NUTTX) +void LoadMon::stack_usage() +{ + unsigned stack_free = 0; + unsigned fds_free = FDS_LOW_WARNING_THRESHOLD + 1; + + bool checked_task = false; + + task_stack_info_s task_stack_info{}; + static_assert(sizeof(task_stack_info.task_name) == CONFIG_TASK_NAME_SIZE, + "task_stack_info.task_name must match NuttX CONFIG_TASK_NAME_SIZE"); + + sched_lock(); + + if (system_load.tasks[_stack_task_index].valid && (system_load.tasks[_stack_task_index].tcb->pid > 0)) { + + stack_free = up_check_tcbstack_remain(system_load.tasks[_stack_task_index].tcb); + + strncpy((char *)task_stack_info.task_name, system_load.tasks[_stack_task_index].tcb->name, CONFIG_TASK_NAME_SIZE - 1); + task_stack_info.task_name[CONFIG_TASK_NAME_SIZE - 1] = '\0'; + + checked_task = true; + +#if CONFIG_NFILE_DESCRIPTORS > 0 + FAR struct task_group_s *group = system_load.tasks[_stack_task_index].tcb->group; + + unsigned tcb_num_used_fds = 0; + + if (group) { + for (int fd_index = 0; fd_index < CONFIG_NFILE_DESCRIPTORS; ++fd_index) { + if (group->tg_filelist.fl_files[fd_index].f_inode) { + ++tcb_num_used_fds; + } + } + + fds_free = CONFIG_NFILE_DESCRIPTORS - tcb_num_used_fds; + } + +#endif // CONFIG_NFILE_DESCRIPTORS + } + + sched_unlock(); + + if (checked_task) { + task_stack_info.stack_free = stack_free; + task_stack_info.timestamp = hrt_absolute_time(); + + _task_stack_info_pub.publish(task_stack_info); + + // Found task low on stack, report and exit. Continue here in next cycle. + if (stack_free < STACK_LOW_WARNING_THRESHOLD) { + PX4_WARN("%s low on stack! (%i bytes left)", task_stack_info.task_name, stack_free); + } + + // Found task low on file descriptors, report and exit. Continue here in next cycle. + if (fds_free < FDS_LOW_WARNING_THRESHOLD) { + PX4_WARN("%s low on FDs! (%i FDs left)", task_stack_info.task_name, fds_free); + } + } + + // Continue after last checked task next cycle + _stack_task_index = (_stack_task_index + 1) % CONFIG_MAX_TASKS; +} +#endif + +int LoadMon::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Background process running periodically on the low priority work queue to calculate the CPU load and RAM +usage and publish the `cpuload` topic. + +On NuttX it also checks the stack usage of each process and if it falls below 300 bytes, a warning is output, +which will also appear in the log file. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("load_mon", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int load_mon_main(int argc, char *argv[]) +{ + return LoadMon::main(argc, argv); +} + +} // namespace load_mon diff --git a/src/prometheus_px4/src/modules/load_mon/LoadMon.hpp b/src/prometheus_px4/src/modules/load_mon/LoadMon.hpp new file mode 100644 index 0000000..afdbb23 --- /dev/null +++ b/src/prometheus_px4/src/modules/load_mon/LoadMon.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 + +#if defined(__PX4_NUTTX) +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(__PX4_LINUX) +#include +#endif + +namespace load_mon +{ + +class LoadMon : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + LoadMon(); + ~LoadMon() override; + + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void start(); + +private: + /** Do a compute and schedule the next cycle. */ + void Run() override; + + /** Do a calculation of the CPU load and publish it. */ + void cpuload(); + + /* Stack check only available on Nuttx */ +#if defined(__PX4_NUTTX) + /* Calculate stack usage */ + void stack_usage(); + + int _stack_task_index{0}; + + uORB::Publication _task_stack_info_pub{ORB_ID(task_stack_info)}; +#endif + uORB::Publication _cpuload_pub {ORB_ID(cpuload)}; + +#if defined(__PX4_LINUX) + FILE *_proc_fd = nullptr; + /* calculate usage directly from clock ticks on Linux */ + clock_t _last_total_time_stamp{}; + clock_t _last_spent_time_stamp{}; +#elif defined(__PX4_NUTTX) + hrt_abstime _last_idle_time {0}; + hrt_abstime _last_idle_time_sample{0}; +#endif + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamBool) _param_sys_stck_en + ) +}; + +} // namespace load_mon diff --git a/src/prometheus_px4/src/modules/load_mon/params.c b/src/prometheus_px4/src/modules/load_mon/params.c new file mode 100644 index 0000000..c2292d9 --- /dev/null +++ b/src/prometheus_px4/src/modules/load_mon/params.c @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Enable stack checking + * + * @boolean + * @group System + */ +PARAM_DEFINE_INT32(SYS_STCK_EN, 1); diff --git a/src/prometheus_px4/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/prometheus_px4/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp new file mode 100644 index 0000000..7ee630e --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -0,0 +1,1079 @@ +#include "BlockLocalPositionEstimator.hpp" +#include +#include +#include +#include +#include + +orb_advert_t mavlink_log_pub = nullptr; + +// required standard deviation of estimate for estimator to publish data +static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m +static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m +static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m + +static const float P_MAX = 1.0e6f; // max allowed value in state covariance +static const float LAND_RATE = 10.0f; // rate of land detector correction + +static const char *msg_label = "[lpe] "; // rate of land detector correction + +BlockLocalPositionEstimator::BlockLocalPositionEstimator() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + + // this block has no parent, and has name LPE + SuperBlock(nullptr, "LPE"), + + // map projection + _map_ref(), + + // flow gyro + _flow_gyro_x_high_pass(this, "FGYRO_HP"), + _flow_gyro_y_high_pass(this, "FGYRO_HP"), + + // stats + _baroStats(this, ""), + _sonarStats(this, ""), + _lidarStats(this, ""), + _flowQStats(this, ""), + _visionStats(this, ""), + _mocapStats(this, ""), + _gpsStats(this, ""), + + // low pass + _xLowPass(this, "X_LP"), + + // use same lp constant for agl + _aglLowPass(this, "X_LP"), + + // delay + _xDelay(this, ""), + _tDelay(this, ""), + + // misc + _timeStamp(hrt_absolute_time()), + _time_origin(0), + _timeStampLastBaro(hrt_absolute_time()), + _time_last_hist(0), + _time_last_flow(0), + _time_last_baro(0), + _time_last_gps(0), + _time_last_lidar(0), + _time_last_sonar(0), + _time_init_sonar(0), + _time_last_vision_p(0), + _time_last_mocap(0), + _time_last_land(0), + _time_last_target(0), + + // reference altitudes + _altOrigin(0), + _altOriginInitialized(false), + _altOriginGlobal(false), + _baroAltOrigin(0), + _gpsAltOrigin(0), + + // status + _receivedGps(false), + _lastArmedState(false), + + // masks + _sensorTimeout(UINT16_MAX), + _sensorFault(0), + _estimatorInitialized(0), + + // sensor update flags + _flowUpdated(false), + _gpsUpdated(false), + _visionUpdated(false), + _mocapUpdated(false), + _lidarUpdated(false), + _sonarUpdated(false), + _landUpdated(false), + _baroUpdated(false), + + // sensor validation flags + _vision_xy_valid(false), + _vision_z_valid(false), + _mocap_xy_valid(false), + _mocap_z_valid(false), + + // sensor std deviations + _vision_eph(0.0), + _vision_epv(0.0), + _mocap_eph(0.0), + _mocap_epv(0.0), + + // local to global coversion related variables + _is_global_cov_init(false), + _ref_lat(0.0), + _ref_lon(0.0), + _ref_alt(0.0) +{ + _sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz (lockstep requires to run at full rate) + + // assign distance subs to array + _dist_subs[0] = &_sub_dist0; + _dist_subs[1] = &_sub_dist1; + _dist_subs[2] = &_sub_dist2; + _dist_subs[3] = &_sub_dist3; + + // initialize A, B, P, x, u + _x.setZero(); + _u.setZero(); + initSS(); + + // map + _map_ref.init_done = false; + + // print fusion settings to console + PX4_INFO("fuse gps: %d, flow: %d, vis_pos: %d, " + "landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, " + "baro: %d\n", + (_param_lpe_fusion.get() & FUSE_GPS) != 0, + (_param_lpe_fusion.get() & FUSE_FLOW) != 0, + (_param_lpe_fusion.get() & FUSE_VIS_POS) != 0, + (_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0, + (_param_lpe_fusion.get() & FUSE_LAND) != 0, + (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0, + (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0, + (_param_lpe_fusion.get() & FUSE_BARO) != 0); +} + +bool +BlockLocalPositionEstimator::init() +{ + if (!_sensors_sub.registerCallback()) { + PX4_ERR("sensor combined callback registration failed!"); + return false; + } + + return true; +} + +Vector BlockLocalPositionEstimator::dynamics( + float t, + const Vector &x, + const Vector &u) +{ + return m_A * x + m_B * u; +} + +void BlockLocalPositionEstimator::Run() +{ + if (should_exit()) { + _sensors_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + if (_vehicle_command_sub.updated()) { + vehicle_command_s vehicle_command; + + if (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) { + const double latitude = vehicle_command.param5; + const double longitude = vehicle_command.param6; + const float altitude = vehicle_command.param7; + + map_projection_init_timestamped(&_global_local_proj_ref, latitude, longitude, vehicle_command.timestamp); + _global_local_alt0 = altitude; + + PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast(altitude)); + } + } + } + + sensor_combined_s imu; + + if (!_sensors_sub.update(&imu)) { + return; + } + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // set dt for all child blocks + setDt(dt); + + // auto-detect connected rangefinders while not armed + _sub_armed.update(); + bool armedState = _sub_armed.get().armed; + + if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) { + // detect distance sensors + for (size_t i = 0; i < N_DIST_SUBS; i++) { + auto *s = _dist_subs[i]; + + if (s == _sub_lidar || s == _sub_sonar) { continue; } + + if (s->update()) { + + if (s->get().timestamp == 0) { continue; } + + if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && + s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && + _sub_lidar == nullptr) { + _sub_lidar = s; + mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i); + + } else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && + s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && + _sub_sonar == nullptr) { + _sub_sonar = s; + mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %zu", msg_label, i); + } + } + } + } + + // reset pos, vel, and terrain on arming + + // XXX this will be re-enabled for indoor use cases using a + // selection param, but is really not helping outdoors + // right now. + + // if (!_lastArmedState && armedState) { + + // // we just armed, we are at origin on the ground + // _x(X_x) = 0; + // _x(X_y) = 0; + // // reset Z or not? _x(X_z) = 0; + + // // we aren't moving, all velocities are zero + // _x(X_vx) = 0; + // _x(X_vy) = 0; + // _x(X_vz) = 0; + + // // assume we are on the ground, so terrain alt is local alt + // _x(X_tz) = _x(X_z); + + // // reset lowpass filter as well + // _xLowPass.setState(_x); + // _aglLowPass.setState(0); + // } + + _lastArmedState = armedState; + + // see which updates are available + const bool paramsUpdated = _parameter_update_sub.updated(); + _baroUpdated = false; + + if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.update()) { + if (_sub_airdata.get().timestamp != _timeStampLastBaro) { + _baroUpdated = true; + _timeStampLastBaro = _sub_airdata.get().timestamp; + } + } + + _flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.update(); + _gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.update(); + _visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.update(); + _mocapUpdated = _sub_mocap_odom.update(); + _lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->update(); + _sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->update(); + _landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate + bool targetPositionUpdated = _sub_landing_target_pose.update(); + + // get new data + _sub_att.update(); + _sub_angular_velocity.update(); + + // update parameters + if (paramsUpdated) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + SuperBlock::updateParams(); + ModuleParams::updateParams(); + updateSSParams(); + } + + // is xy valid? + bool vxy_stddev_ok = false; + + if (math::max(m_P(X_vx, X_vx), m_P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get()) { + vxy_stddev_ok = true; + } + + if (_estimatorInitialized & EST_XY) { + // if valid and gps has timed out, set to not valid + if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) { + _estimatorInitialized &= ~EST_XY; + } + + } else { + if (vxy_stddev_ok) { + if (!(_sensorTimeout & SENSOR_GPS) + || !(_sensorTimeout & SENSOR_FLOW) + || !(_sensorTimeout & SENSOR_VISION) + || !(_sensorTimeout & SENSOR_MOCAP) + || !(_sensorTimeout & SENSOR_LAND) + || !(_sensorTimeout & SENSOR_LAND_TARGET) + ) { + _estimatorInitialized |= EST_XY; + } + } + } + + // is z valid? + bool z_stddev_ok = sqrtf(m_P(X_z, X_z)) < _param_lpe_z_pub.get(); + + if (_estimatorInitialized & EST_Z) { + // if valid and baro has timed out, set to not valid + if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) { + _estimatorInitialized &= ~EST_Z; + } + + } else { + if (z_stddev_ok) { + _estimatorInitialized |= EST_Z; + } + } + + // is terrain valid? + bool tz_stddev_ok = sqrtf(m_P(X_tz, X_tz)) < _param_lpe_z_pub.get(); + + if (_estimatorInitialized & EST_TZ) { + if (!tz_stddev_ok) { + _estimatorInitialized &= ~EST_TZ; + } + + } else { + if (tz_stddev_ok) { + _estimatorInitialized |= EST_TZ; + } + } + + // check timeouts + checkTimeouts(); + + // if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters + if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get()) { + map_projection_init(&_map_ref, + (double)_param_lpe_lat.get(), + (double)_param_lpe_lon.get()); + + // set timestamp when origin was set to current time + _time_origin = _timeStamp; + + mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m", + double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin)); + } + + // reinitialize x if necessary + bool reinit_x = false; + + for (size_t i = 0; i < n_x; i++) { + // should we do a reinit + // of sensors here? + // don't want it to take too long + if (!PX4_ISFINITE(_x(i))) { + reinit_x = true; + mavlink_log_info(&mavlink_log_pub, "%sreinit x, x(%zu) not finite", msg_label, i); + break; + } + } + + if (reinit_x) { + for (size_t i = 0; i < n_x; i++) { + _x(i) = 0; + } + + mavlink_log_info(&mavlink_log_pub, "%sreinit x", msg_label); + } + + // force P symmetry and reinitialize P if necessary + bool reinit_P = false; + + for (size_t i = 0; i < n_x; i++) { + for (size_t j = 0; j <= i; j++) { + if (!PX4_ISFINITE(m_P(i, j))) { + mavlink_log_info(&mavlink_log_pub, + "%sreinit P (%zu, %zu) not finite", msg_label, i, j); + reinit_P = true; + } + + if (i == j) { + // make sure diagonal elements are positive + if (m_P(i, i) <= 0) { + mavlink_log_info(&mavlink_log_pub, + "%sreinit P (%zu, %zu) negative", msg_label, i, j); + reinit_P = true; + } + + } else { + // copy elememnt from upper triangle to force + // symmetry + m_P(j, i) = m_P(i, j); + } + + if (reinit_P) { break; } + } + + if (reinit_P) { break; } + } + + if (reinit_P) { + initP(); + } + + // do prediction + predict(imu); + + // sensor corrections/ initializations + if (_gpsUpdated) { + if (_sensorTimeout & SENSOR_GPS) { + gpsInit(); + + } else { + gpsCorrect(); + } + } + + if (_baroUpdated) { + if (_sensorTimeout & SENSOR_BARO) { + baroInit(); + + } else { + baroCorrect(); + } + } + + if (_lidarUpdated) { + if (_sensorTimeout & SENSOR_LIDAR) { + lidarInit(); + + } else { + lidarCorrect(); + } + } + + if (_sonarUpdated) { + if (_sensorTimeout & SENSOR_SONAR) { + sonarInit(); + + } else { + sonarCorrect(); + } + } + + if (_flowUpdated) { + if (_sensorTimeout & SENSOR_FLOW) { + flowInit(); + + } else { + flowCorrect(); + } + } + + if (_visionUpdated) { + if (_sensorTimeout & SENSOR_VISION) { + visionInit(); + + } else { + visionCorrect(); + } + } + + if (_mocapUpdated) { + if (_sensorTimeout & SENSOR_MOCAP) { + mocapInit(); + + } else { + mocapCorrect(); + } + } + + if (_landUpdated) { + if (_sensorTimeout & SENSOR_LAND) { + landInit(); + + } else { + landCorrect(); + } + } + + if (targetPositionUpdated) { + if (_sensorTimeout & SENSOR_LAND_TARGET) { + landingTargetInit(); + + } else { + landingTargetCorrect(); + } + } + + if (_altOriginInitialized) { + // update all publications if possible + publishLocalPos(); + publishOdom(); + publishEstimatorStatus(); + + _pub_innov.get().timestamp_sample = _timeStamp; + _pub_innov.get().timestamp = hrt_absolute_time(); + _pub_innov.update(); + + _pub_innov_var.get().timestamp_sample = _timeStamp; + _pub_innov_var.get().timestamp = hrt_absolute_time(); + _pub_innov_var.update(); + + if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) { + publishGlobalPos(); + } + } + + // propagate delayed state, no matter what + // if state is frozen, delayed state still + // needs to be propagated with frozen state + float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist); + + if (_time_last_hist == 0 || + (dt_hist > HIST_STEP)) { + _tDelay.update(Scalar(_timeStamp)); + _xDelay.update(_x); + _time_last_hist = _timeStamp; + } +} + +void BlockLocalPositionEstimator::checkTimeouts() +{ + baroCheckTimeout(); + gpsCheckTimeout(); + lidarCheckTimeout(); + flowCheckTimeout(); + sonarCheckTimeout(); + visionCheckTimeout(); + mocapCheckTimeout(); + landCheckTimeout(); + landingTargetCheckTimeout(); +} + +bool BlockLocalPositionEstimator::landed() +{ + if (!(_param_lpe_fusion.get() & FUSE_LAND)) { + return false; + } + + _sub_land.update(); + + bool disarmed_not_falling = (!_sub_armed.get().armed) && (!_sub_land.get().freefall); + + return _sub_land.get().landed || disarmed_not_falling; +} + +void BlockLocalPositionEstimator::publishLocalPos() +{ + const Vector &xLP = _xLowPass.getState(); + + // lie about eph/epv to allow visual odometry only navigation when velocity est. good + float evh = sqrtf(m_P(X_vx, X_vx) + m_P(X_vy, X_vy)); + float evv = sqrtf(m_P(X_vz, X_vz)); + float eph = sqrtf(m_P(X_x, X_x) + m_P(X_y, X_y)); + float epv = sqrtf(m_P(X_z, X_z)); + + float eph_thresh = 3.0f; + float epv_thresh = 3.0f; + + if (evh < _param_lpe_vxy_pub.get()) { + if (eph > eph_thresh) { + eph = eph_thresh; + } + + if (epv > epv_thresh) { + epv = epv_thresh; + } + } + + // publish local position + if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) && + PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) + && PX4_ISFINITE(_x(X_vz))) { + _pub_lpos.get().timestamp_sample = _timeStamp; + + _pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY; + _pub_lpos.get().z_valid = _estimatorInitialized & EST_Z; + _pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY; + _pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z; + + _pub_lpos.get().x = xLP(X_x); // north + _pub_lpos.get().y = xLP(X_y); // east + + if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) { + _pub_lpos.get().z = -_aglLowPass.getState(); // agl + + } else { + _pub_lpos.get().z = xLP(X_z); // down + } + + _pub_lpos.get().heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); + + _pub_lpos.get().vx = xLP(X_vx); // north + _pub_lpos.get().vy = xLP(X_vy); // east + _pub_lpos.get().vz = xLP(X_vz); // down + + // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity + _pub_lpos.get().z_deriv = xLP(X_vz); + + _pub_lpos.get().ax = _u(U_ax); // north + _pub_lpos.get().ay = _u(U_ay); // east + _pub_lpos.get().az = _u(U_az); // down + + _pub_lpos.get().xy_global = _estimatorInitialized & EST_XY; + _pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal; + _pub_lpos.get().ref_timestamp = _time_origin; + _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI; + _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; + _pub_lpos.get().ref_alt = _altOrigin; + _pub_lpos.get().dist_bottom = _aglLowPass.getState(); + // we estimate agl even when we don't have terrain info + // if you are in terrain following mode this is important + // so that if terrain estimation fails there isn't a + // sudden altitude jump + _pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z; + _pub_lpos.get().eph = eph; + _pub_lpos.get().epv = epv; + _pub_lpos.get().evh = evh; + _pub_lpos.get().evv = evv; + _pub_lpos.get().vxy_max = INFINITY; + _pub_lpos.get().vz_max = INFINITY; + _pub_lpos.get().hagl_min = INFINITY; + _pub_lpos.get().hagl_max = INFINITY; + _pub_lpos.get().timestamp = hrt_absolute_time();; + _pub_lpos.update(); + } +} + +void BlockLocalPositionEstimator::publishOdom() +{ + const Vector &xLP = _xLowPass.getState(); + + // publish vehicle odometry + if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) && + PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) + && PX4_ISFINITE(_x(X_vz))) { + + _pub_odom.get().timestamp_sample = _timeStamp; + _pub_odom.get().local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + // position + _pub_odom.get().x = xLP(X_x); // north + _pub_odom.get().y = xLP(X_y); // east + + if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) { + _pub_odom.get().z = -_aglLowPass.getState(); // agl + + } else { + _pub_odom.get().z = xLP(X_z); // down + } + + // orientation + matrix::Quatf q = matrix::Quatf(_sub_att.get().q); + q.copyTo(_pub_odom.get().q); + + // linear velocity + _pub_odom.get().velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; + _pub_odom.get().vx = xLP(X_vx); // vel north + _pub_odom.get().vy = xLP(X_vy); // vel east + _pub_odom.get().vz = xLP(X_vz); // vel down + + // angular velocity + _pub_odom.get().rollspeed = _sub_angular_velocity.get().xyz[0]; // roll rate + _pub_odom.get().pitchspeed = _sub_angular_velocity.get().xyz[1]; // pitch rate + _pub_odom.get().yawspeed = _sub_angular_velocity.get().xyz[2]; // yaw rate + + // get the covariance matrix size + const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]); + const size_t VEL_URT_SIZE = sizeof(_pub_odom.get().velocity_covariance) / sizeof( + _pub_odom.get().velocity_covariance[0]); + + // initially set pose covariances to 0 + for (size_t i = 0; i < POS_URT_SIZE; i++) { + _pub_odom.get().pose_covariance[i] = 0.0; + } + + // set the position variances + _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = m_P(X_vx, X_vx); + _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = m_P(X_vy, X_vy); + _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = m_P(X_vz, X_vz); + + // unknown orientation covariances + // TODO: add orientation covariance to vehicle_attitude + _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLL_VARIANCE] = NAN; + _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCH_VARIANCE] = NAN; + _pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAW_VARIANCE] = NAN; + + // initially set velocity covariances to 0 + for (size_t i = 0; i < VEL_URT_SIZE; i++) { + _pub_odom.get().velocity_covariance[i] = 0.0; + } + + // set the linear velocity variances + _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = m_P(X_vx, X_vx); + _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = m_P(X_vy, X_vy); + _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = m_P(X_vz, X_vz); + + // unknown angular velocity covariances + _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLLRATE_VARIANCE] = NAN; + _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCHRATE_VARIANCE] = NAN; + _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAWRATE_VARIANCE] = NAN; + + _pub_odom.get().timestamp = hrt_absolute_time(); + _pub_odom.update(); + } +} + +void BlockLocalPositionEstimator::publishEstimatorStatus() +{ + _pub_est_states.get().timestamp_sample = _timeStamp; + + for (size_t i = 0; i < n_x; i++) { + _pub_est_states.get().states[i] = _x(i); + } + + // matching EKF2 covariances indexing + // quaternion - not determined, as it is a position estimator + _pub_est_states.get().covariances[0] = NAN; + _pub_est_states.get().covariances[1] = NAN; + _pub_est_states.get().covariances[2] = NAN; + _pub_est_states.get().covariances[3] = NAN; + // linear velocity + _pub_est_states.get().covariances[4] = m_P(X_vx, X_vx); + _pub_est_states.get().covariances[5] = m_P(X_vy, X_vy); + _pub_est_states.get().covariances[6] = m_P(X_vz, X_vz); + // position + _pub_est_states.get().covariances[7] = m_P(X_x, X_x); + _pub_est_states.get().covariances[8] = m_P(X_y, X_y); + _pub_est_states.get().covariances[9] = m_P(X_z, X_z); + // gyro bias - not determined + _pub_est_states.get().covariances[10] = NAN; + _pub_est_states.get().covariances[11] = NAN; + _pub_est_states.get().covariances[12] = NAN; + // accel bias + _pub_est_states.get().covariances[13] = m_P(X_bx, X_bx); + _pub_est_states.get().covariances[14] = m_P(X_by, X_by); + _pub_est_states.get().covariances[15] = m_P(X_bz, X_bz); + + // mag - not determined + for (size_t i = 16; i <= 21; i++) { + _pub_est_states.get().covariances[i] = NAN; + } + + // replacing the hor wind cov with terrain altitude covariance + _pub_est_states.get().covariances[22] = m_P(X_tz, X_tz); + _pub_est_states.get().covariances[23] = NAN; + + _pub_est_states.get().n_states = n_x; + _pub_est_states.get().timestamp = hrt_absolute_time(); + _pub_est_states.update(); + + // estimator_status + _pub_est_status.get().timestamp_sample = _timeStamp; + _pub_est_status.get().health_flags = _sensorFault; + _pub_est_status.get().timeout_flags = _sensorTimeout; + _pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph; + _pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv; + + _pub_est_status.get().timestamp = hrt_absolute_time(); + _pub_est_status.update(); +} + +void BlockLocalPositionEstimator::publishGlobalPos() +{ + // publish global position + double lat = 0; + double lon = 0; + const Vector &xLP = _xLowPass.getState(); + map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon); + float alt = -xLP(X_z) + _altOrigin; + + // lie about eph/epv to allow visual odometry only navigation when velocity est. good + float evh = sqrtf(m_P(X_vx, X_vx) + m_P(X_vy, X_vy)); + float eph = sqrtf(m_P(X_x, X_x) + m_P(X_y, X_y)); + float epv = sqrtf(m_P(X_z, X_z)); + + float eph_thresh = 3.0f; + float epv_thresh = 3.0f; + + if (evh < _param_lpe_vxy_pub.get()) { + if (eph > eph_thresh) { + eph = eph_thresh; + } + + if (epv > epv_thresh) { + epv = epv_thresh; + } + } + + if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && + PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) && + PX4_ISFINITE(xLP(X_vz))) { + _pub_gpos.get().timestamp_sample = _timeStamp; + _pub_gpos.get().lat = lat; + _pub_gpos.get().lon = lon; + _pub_gpos.get().alt = alt; + _pub_gpos.get().eph = eph; + _pub_gpos.get().epv = epv; + _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); + _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ; + _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY); + _pub_gpos.get().timestamp = hrt_absolute_time(); + _pub_gpos.update(); + } +} + +void BlockLocalPositionEstimator::initP() +{ + m_P.setZero(); + // initialize to twice valid condition + m_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID; + m_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID; + m_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID; + m_P(X_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get(); + m_P(X_vy, X_vy) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get(); + // use vxy thresh for vz init as well + m_P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get(); + // initialize bias uncertainty to small values to keep them stable + m_P(X_bx, X_bx) = 1e-6; + m_P(X_by, X_by) = 1e-6; + m_P(X_bz, X_bz) = 1e-6; + m_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID; +} + +void BlockLocalPositionEstimator::initSS() +{ + initP(); + + // dynamics matrix + m_A.setZero(); + // derivative of position is velocity + m_A(X_x, X_vx) = 1; + m_A(X_y, X_vy) = 1; + m_A(X_z, X_vz) = 1; + + // input matrix + m_B.setZero(); + m_B(X_vx, U_ax) = 1; + m_B(X_vy, U_ay) = 1; + m_B(X_vz, U_az) = 1; + + // update components that depend on current state + updateSSStates(); + updateSSParams(); +} + +void BlockLocalPositionEstimator::updateSSStates() +{ + // derivative of velocity is accelerometer acceleration + // (in input matrix) - bias (in body frame) + m_A(X_vx, X_bx) = -_R_att(0, 0); + m_A(X_vx, X_by) = -_R_att(0, 1); + m_A(X_vx, X_bz) = -_R_att(0, 2); + + m_A(X_vy, X_bx) = -_R_att(1, 0); + m_A(X_vy, X_by) = -_R_att(1, 1); + m_A(X_vy, X_bz) = -_R_att(1, 2); + + m_A(X_vz, X_bx) = -_R_att(2, 0); + m_A(X_vz, X_by) = -_R_att(2, 1); + m_A(X_vz, X_bz) = -_R_att(2, 2); +} + +void BlockLocalPositionEstimator::updateSSParams() +{ + // input noise covariance matrix + m_R.setZero(); + m_R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get(); + m_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get(); + m_R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get(); + + // process noise power matrix + m_Q.setZero(); + float pn_p_sq = _param_lpe_pn_p.get() * _param_lpe_pn_p.get(); + float pn_v_sq = _param_lpe_pn_v.get() * _param_lpe_pn_v.get(); + m_Q(X_x, X_x) = pn_p_sq; + m_Q(X_y, X_y) = pn_p_sq; + m_Q(X_z, X_z) = pn_p_sq; + m_Q(X_vx, X_vx) = pn_v_sq; + m_Q(X_vy, X_vy) = pn_v_sq; + m_Q(X_vz, X_vz) = pn_v_sq; + + // technically, the noise is in the body frame, + // but the components are all the same, so + // ignoring for now + float pn_b_sq = _param_lpe_pn_b.get() * _param_lpe_pn_b.get(); + m_Q(X_bx, X_bx) = pn_b_sq; + m_Q(X_by, X_by) = pn_b_sq; + m_Q(X_bz, X_bz) = pn_b_sq; + + // terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity + float pn_t_noise_density = + _param_lpe_pn_t.get() + + (_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy)); + m_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density; +} + +void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu) +{ + // get acceleration + _R_att = matrix::Dcm(matrix::Quatf(_sub_att.get().q)); + Vector3f a(imu.accelerometer_m_s2); + // note, bias is removed in dynamics function + _u = _R_att * a; + _u(U_az) += CONSTANTS_ONE_G; // add g + + // update state space based on new states + updateSSStates(); + + // continuous time kalman filter prediction + // integrate runge kutta 4th order + // TODO move rk4 algorithm to matrixlib + // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods + float h = getDt(); + Vector k1, k2, k3, k4; + k1 = dynamics(0, _x, _u); + k2 = dynamics(h / 2, _x + k1 * h / 2, _u); + k3 = dynamics(h / 2, _x + k2 * h / 2, _u); + k4 = dynamics(h, _x + k3 * h, _u); + Vector dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6); + + // don't integrate position if no valid xy data + if (!(_estimatorInitialized & EST_XY)) { + dx(X_x) = 0; + dx(X_vx) = 0; + dx(X_y) = 0; + dx(X_vy) = 0; + } + + // don't integrate z if no valid z data + if (!(_estimatorInitialized & EST_Z)) { + dx(X_z) = 0; + } + + // don't integrate tz if no valid tz data + if (!(_estimatorInitialized & EST_TZ)) { + dx(X_tz) = 0; + } + + // saturate bias + float bx = dx(X_bx) + _x(X_bx); + float by = dx(X_by) + _x(X_by); + float bz = dx(X_bz) + _x(X_bz); + + if (std::abs(bx) > BIAS_MAX) { + bx = BIAS_MAX * bx / std::abs(bx); + dx(X_bx) = bx - _x(X_bx); + } + + if (std::abs(by) > BIAS_MAX) { + by = BIAS_MAX * by / std::abs(by); + dx(X_by) = by - _x(X_by); + } + + if (std::abs(bz) > BIAS_MAX) { + bz = BIAS_MAX * bz / std::abs(bz); + dx(X_bz) = bz - _x(X_bz); + } + + // propagate + _x += dx; + Matrix dP = (m_A * m_P + m_P * m_A.transpose() + + m_B * m_R * m_B.transpose() + m_Q) * getDt(); + + // covariance propagation logic + for (size_t i = 0; i < n_x; i++) { + if (m_P(i, i) > P_MAX) { + // if diagonal element greater than max, stop propagating + dP(i, i) = 0; + + for (size_t j = 0; j < n_x; j++) { + dP(i, j) = 0; + dP(j, i) = 0; + } + } + } + + m_P += dP; + _xLowPass.update(_x); + _aglLowPass.update(agl()); +} + +int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods) +{ + float t_delay = 0; + uint8_t i_hist = 0; + + for (i_hist = 1; i_hist < HIST_LEN; i_hist++) { + t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0)); + + if (t_delay > delay) { + break; + } + } + + *periods = i_hist; + + if (t_delay > DELAY_MAX) { + mavlink_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay)); + return -1; + } + + return OK; +} + +int +BlockLocalPositionEstimator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int +BlockLocalPositionEstimator::task_spawn(int argc, char *argv[]) +{ + BlockLocalPositionEstimator *instance = new BlockLocalPositionEstimator(); + + 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 +BlockLocalPositionEstimator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Attitude and position estimator using an Extended Kalman Filter. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("local_position_estimator", "estimator"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]) +{ + return BlockLocalPositionEstimator::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/prometheus_px4/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp new file mode 100644 index 0000000..597d0e8 --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -0,0 +1,468 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +// uORB Subscriptions +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// uORB Publications +#include +#include +#include +#include +#include +#include + +using namespace matrix; +using namespace control; +using namespace time_literals; + +static const float DELAY_MAX = 0.5f; // seconds +static const float HIST_STEP = 0.05f; // 20 hz +static const float BIAS_MAX = 1e-1f; +static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP; +static const size_t N_DIST_SUBS = 4; + +// for fault detection +// chi squared distribution, false alarm probability 0.0001 +// see fault_table.py +// note skip 0 index so we can use degree of freedom as index +static const float BETA_TABLE[7] = {0, + 8.82050518214, + 12.094592431, + 13.9876612368, + 16.0875642296, + 17.8797700658, + 19.6465647819, + }; + +class BlockLocalPositionEstimator : public ModuleBase, public ModuleParams, + public px4::WorkItem, public control::SuperBlock +{ +// dynamics: +// +// x(+) = A * x(-) + B * u(+) +// y_i = C_i*x +// +// kalman filter +// +// E[xx'] = P +// E[uu'] = W +// E[y_iy_i'] = R_i +// +// prediction +// x(+|-) = A*x(-|-) + B*u(+) +// P(+|-) = A*P(-|-)*A' + B*W*B' +// +// correction +// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) ) +// +// +// input: +// ax, ay, az (acceleration NED) +// +// states: +// px, py, pz , ( position NED, m) +// vx, vy, vz ( vel NED, m/s), +// bx, by, bz ( accel bias, m/s^2) +// tz (terrain altitude, ASL, m) +// +// measurements: +// +// sonar: pz (measured d*cos(phi)*cos(theta)) +// +// baro: pz +// +// flow: vx, vy (flow is in body x, y frame) +// +// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame) +// +// lidar: pz (actual measured d*cos(phi)*cos(theta)) +// +// vision: px, py, pz, vx, vy, vz +// +// mocap: px, py, pz +// +// land (detects when landed)): pz (always measures agl = 0) +// +public: + + BlockLocalPositionEstimator(); + ~BlockLocalPositionEstimator() 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: + + // constants + enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x}; + enum {U_ax = 0, U_ay, U_az, n_u}; + enum {Y_baro_z = 0, n_y_baro}; + enum {Y_lidar_z = 0, n_y_lidar}; + enum {Y_flow_vx = 0, Y_flow_vy, n_y_flow}; + enum {Y_sonar_z = 0, n_y_sonar}; + enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps}; + enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision}; + enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap}; + enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land}; + enum {Y_target_x = 0, Y_target_y, n_y_target}; + enum { + FUSE_GPS = 1 << 0, + FUSE_FLOW = 1 << 1, + FUSE_VIS_POS = 1 << 2, + FUSE_LAND_TARGET = 1 << 3, + FUSE_LAND = 1 << 4, + FUSE_PUB_AGL_Z = 1 << 5, + FUSE_FLOW_GYRO_COMP = 1 << 6, + FUSE_BARO = 1 << 7 + }; + + enum sensor_t { + SENSOR_BARO = 1 << 0, + SENSOR_GPS = 1 << 1, + SENSOR_LIDAR = 1 << 2, + SENSOR_FLOW = 1 << 3, + SENSOR_SONAR = 1 << 4, + SENSOR_VISION = 1 << 5, + SENSOR_MOCAP = 1 << 6, + SENSOR_LAND = 1 << 7, + SENSOR_LAND_TARGET = 1 << 8, + }; + + enum estimate_t { + EST_XY = 1 << 0, + EST_Z = 1 << 1, + EST_TZ = 1 << 2, + }; + + void Run() override; + + // methods + // ---------------------------- + // + Vector dynamics( + float t, + const Vector &x, + const Vector &u); + void initP(); + void initSS(); + void updateSSStates(); + void updateSSParams(); + + // predict the next state + void predict(const sensor_combined_s &imu); + + // lidar + int lidarMeasure(Vector &y); + void lidarCorrect(); + void lidarInit(); + void lidarCheckTimeout(); + + // sonar + int sonarMeasure(Vector &y); + void sonarCorrect(); + void sonarInit(); + void sonarCheckTimeout(); + + // baro + int baroMeasure(Vector &y); + void baroCorrect(); + void baroInit(); + void baroCheckTimeout(); + + // gps + int gpsMeasure(Vector &y); + void gpsCorrect(); + void gpsInit(); + void gpsCheckTimeout(); + + // flow + int flowMeasure(Vector &y); + void flowCorrect(); + void flowInit(); + void flowCheckTimeout(); + + // vision + int visionMeasure(Vector &y); + void visionCorrect(); + void visionInit(); + void visionCheckTimeout(); + + // mocap + int mocapMeasure(Vector &y); + void mocapCorrect(); + void mocapInit(); + void mocapCheckTimeout(); + + // land + int landMeasure(Vector &y); + void landCorrect(); + void landInit(); + void landCheckTimeout(); + + // landing target + int landingTargetMeasure(Vector &y); + void landingTargetCorrect(); + void landingTargetInit(); + void landingTargetCheckTimeout(); + + // timeouts + void checkTimeouts(); + + // misc + inline float agl() + { + return _x(X_tz) - _x(X_z); + } + bool landed(); + int getDelayPeriods(float delay, uint8_t *periods); + + // publications + void publishLocalPos(); + void publishGlobalPos(); + void publishOdom(); + void publishEstimatorStatus(); + void publishEk2fTimestamps(); + + // attributes + // ---------------------------- + + // subscriptions + uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::SubscriptionData _sub_armed{ORB_ID(actuator_armed)}; + uORB::SubscriptionData _sub_land{ORB_ID(vehicle_land_detected)}; + uORB::SubscriptionData _sub_att{ORB_ID(vehicle_attitude)}; + uORB::SubscriptionData _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)}; + uORB::SubscriptionData _sub_flow{ORB_ID(optical_flow)}; + uORB::SubscriptionData _sub_gps{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionData _sub_visual_odom{ORB_ID(vehicle_visual_odometry)}; + uORB::SubscriptionData _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)}; + uORB::SubscriptionData _sub_dist0{ORB_ID(distance_sensor), 0}; + uORB::SubscriptionData _sub_dist1{ORB_ID(distance_sensor), 1}; + uORB::SubscriptionData _sub_dist2{ORB_ID(distance_sensor), 2}; + uORB::SubscriptionData _sub_dist3{ORB_ID(distance_sensor), 3}; + uORB::SubscriptionData *_dist_subs[N_DIST_SUBS] {}; + uORB::SubscriptionData *_sub_lidar{nullptr}; + uORB::SubscriptionData *_sub_sonar{nullptr}; + uORB::SubscriptionData _sub_landing_target_pose{ORB_ID(landing_target_pose)}; + uORB::SubscriptionData _sub_airdata{ORB_ID(vehicle_air_data)}; + + // publications + uORB::PublicationData _pub_lpos{ORB_ID(vehicle_local_position)}; + uORB::PublicationData _pub_gpos{ORB_ID(vehicle_global_position)}; + uORB::PublicationData _pub_odom{ORB_ID(vehicle_odometry)}; + uORB::PublicationData _pub_est_states{ORB_ID(estimator_states)}; + uORB::PublicationData _pub_est_status{ORB_ID(estimator_status)}; + uORB::PublicationData _pub_innov{ORB_ID(estimator_innovations)}; + uORB::PublicationData _pub_innov_var{ORB_ID(estimator_innovation_variances)}; + + // map projection + struct map_projection_reference_s _map_ref; + + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + + // target mode paramters from landing_target_estimator module + enum TargetMode { + Target_Moving = 0, + Target_Stationary = 1 + }; + + // flow gyro filter + BlockHighPass _flow_gyro_x_high_pass; + BlockHighPass _flow_gyro_y_high_pass; + + // stats + BlockStats _baroStats; + BlockStats _sonarStats; + BlockStats _lidarStats; + BlockStats _flowQStats; + BlockStats _visionStats; + BlockStats _mocapStats; + BlockStats _gpsStats; + uint16_t _landCount; + + // low pass + BlockLowPassVector _xLowPass; + BlockLowPass _aglLowPass; + + // delay blocks + BlockDelay _xDelay; + BlockDelay _tDelay; + + // misc + uint64_t _timeStamp; + uint64_t _time_origin; + uint64_t _timeStampLastBaro; + uint64_t _time_last_hist; + uint64_t _time_last_flow; + uint64_t _time_last_baro; + uint64_t _time_last_gps; + uint64_t _time_last_lidar; + uint64_t _time_last_sonar; + uint64_t _time_init_sonar; + uint64_t _time_last_vision_p; + uint64_t _time_last_mocap; + uint64_t _time_last_land; + uint64_t _time_last_target; + + // reference altitudes + float _altOrigin; + bool _altOriginInitialized; + bool _altOriginGlobal; // true when the altitude of the origin is defined wrt a global reference frame + float _baroAltOrigin; + float _gpsAltOrigin; + + // status + bool _receivedGps; + bool _lastArmedState; + + // masks + uint16_t _sensorTimeout; + uint16_t _sensorFault; + uint8_t _estimatorInitialized; + + // sensor update flags + bool _flowUpdated; + bool _gpsUpdated; + bool _visionUpdated; + bool _mocapUpdated; + bool _lidarUpdated; + bool _sonarUpdated; + bool _landUpdated; + bool _baroUpdated; + + // sensor validation flags + bool _vision_xy_valid; + bool _vision_z_valid; + bool _mocap_xy_valid; + bool _mocap_z_valid; + + // sensor std deviations + float _vision_eph; + float _vision_epv; + float _mocap_eph; + float _mocap_epv; + + // local to global coversion related variables + bool _is_global_cov_init; + double _ref_lat; + double _ref_lon; + float _ref_alt; + + // state space + Vector _x; // state vector + Vector _u; // input vector + Matrix m_P; // state covariance matrix + + matrix::Dcm _R_att; + + Matrix m_A; // dynamics matrix + Matrix m_B; // input matrix + Matrix m_R; // input covariance + Matrix m_Q; // process noise covariance + + + DEFINE_PARAMETERS( + // general parameters + (ParamInt) _param_lpe_fusion, + (ParamFloat) _param_lpe_vxy_pub, + (ParamFloat) _param_lpe_z_pub, + + // sonar parameters + (ParamFloat) _param_lpe_snr_z, + (ParamFloat) _param_lpe_snr_off_z, + + // lidar parameters + (ParamFloat) _param_lpe_ldr_z, + (ParamFloat) _param_lpe_ldr_off_z, + + // accel parameters + (ParamFloat) _param_lpe_acc_xy, + (ParamFloat) _param_lpe_acc_z, + + // baro parameters + (ParamFloat) _param_lpe_bar_z, + + // gps parameters + (ParamFloat) _param_lpe_gps_delay, + (ParamFloat) _param_lpe_gps_xy, + (ParamFloat) _param_lpe_gps_z, + (ParamFloat) _param_lpe_gps_vxy, + (ParamFloat) _param_lpe_gps_vz, + (ParamFloat) _param_lpe_eph_max, + (ParamFloat) _param_lpe_epv_max, + + // vision parameters + (ParamFloat) _param_lpe_vis_xy, + (ParamFloat) _param_lpe_vis_z, + (ParamFloat) _param_lpe_vis_delay, + + // mocap parameters + (ParamFloat) _param_lpe_vic_p, + + // flow parameters + (ParamFloat) _param_lpe_flw_off_z, + (ParamFloat) _param_lpe_flw_scale, + (ParamInt) _param_lpe_flw_qmin, + (ParamFloat) _param_lpe_flw_r, + (ParamFloat) _param_lpe_flw_rr, + + // land parameters + (ParamFloat) _param_lpe_land_z, + (ParamFloat) _param_lpe_land_vxy, + + // process noise + (ParamFloat) _param_lpe_pn_p, + (ParamFloat) _param_lpe_pn_v, + (ParamFloat) _param_lpe_pn_b, + (ParamFloat) _param_lpe_pn_t, + (ParamFloat) _param_lpe_t_max_grade, + + (ParamFloat) _param_lpe_lt_cov, + (ParamInt) _param_ltest_mode, + + // init origin + (ParamInt) _param_lpe_fake_origin, + (ParamFloat) _param_lpe_lat, + (ParamFloat) _param_lpe_lon + ) + +}; diff --git a/src/prometheus_px4/src/modules/local_position_estimator/CMakeLists.txt b/src/prometheus_px4/src/modules/local_position_estimator/CMakeLists.txt new file mode 100644 index 0000000..a54534e --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# 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__local_position_estimator + MAIN local_position_estimator + COMPILE_FLAGS + STACK_MAIN 5700 + STACK_MAX 3700 + SRCS + BlockLocalPositionEstimator.cpp + sensors/flow.cpp + sensors/lidar.cpp + sensors/sonar.cpp + sensors/gps.cpp + sensors/baro.cpp + sensors/vision.cpp + sensors/mocap.cpp + sensors/land.cpp + sensors/landing_target.cpp + DEPENDS + controllib + git_ecl + ecl_geo + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/local_position_estimator/fault_table.py b/src/prometheus_px4/src/modules/local_position_estimator/fault_table.py new file mode 100644 index 0000000..9c84074 --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/fault_table.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python +from __future__ import print_function +import pylab as pl +import scipy.optimize +from scipy.stats import chi2 + +for fa_rate in 1.0/pl.array([1e1, 1e2, 1e4, 1e6, 1e9]): + print(fa_rate) + for df in range(1,7): + f_eq = lambda x: ((1- fa_rate) - chi2.cdf(x, df))**2 + res = scipy.optimize.minimize(f_eq, df) + assert res['success'] + print('\t', res.x[0]) diff --git a/src/prometheus_px4/src/modules/local_position_estimator/params.c b/src/prometheus_px4/src/modules/local_position_estimator/params.c new file mode 100644 index 0000000..a24a499 --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/params.c @@ -0,0 +1,468 @@ +#include + +// 16 is max name length + +/** + * Optical flow z offset from center + * + * @group Local Position Estimator + * @unit m + * @min -1 + * @max 1 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f); + +/** + * Optical flow scale + * + * @group Local Position Estimator + * @unit m + * @min 0.1 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_SCALE, 1.3f); + +/** + * Optical flow rotation (roll/pitch) noise gain + * + * @group Local Position Estimator + * @unit m/s/rad + * @min 0.1 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_R, 7.0f); + +/** + * Optical flow angular velocity noise gain + * + * @group Local Position Estimator + * @unit m/rad + * @min 0.0 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_RR, 7.0f); + +/** + * Optical flow minimum quality threshold + * + * @group Local Position Estimator + * @min 0 + * @max 255 + * @decimal 0 + */ +PARAM_DEFINE_INT32(LPE_FLW_QMIN, 150); + +/** + * Sonar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.05f); + +/** + * Sonar z offset from center of vehicle +down + * + * @group Local Position Estimator + * @unit m + * @min -1 + * @max 1 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_SNR_OFF_Z, 0.00f); + +/** + * Lidar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f); + +/** + * Lidar z offset from center of vehicle +down + * + * @group Local Position Estimator + * @unit m + * @min -1 + * @max 1 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f); + +/** + * Accelerometer xy noise density + * + * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz) + * + * Larger than data sheet to account for tilt error. + * + * @group Local Position Estimator + * @unit m/s^2/sqrt(Hz) + * @min 0.00001 + * @max 2 + * @decimal 4 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.012f); + +/** + * Accelerometer z noise density + * + * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz) + * + * @group Local Position Estimator + * @unit m/s^2/sqrt(Hz) + * @min 0.00001 + * @max 2 + * @decimal 4 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.02f); + +/** + * Barometric presssure altitude z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 100 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_BAR_Z, 3.0f); + +/** + * GPS delay compensaton + * + * @group Local Position Estimator + * @unit s + * @min 0 + * @max 0.4 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.29f); + + +/** + * Minimum GPS xy standard deviation, uses reported EPH if greater. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 5 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_XY, 1.0f); + +/** + * Minimum GPS z standard deviation, uses reported EPV if greater. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 200 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_Z, 3.0f); + +/** + * GPS xy velocity standard deviation. + * + * EPV used if greater than this value. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.25f); + +/** + * GPS z velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f); + +/** + * Max EPH allowed for GPS initialization + * + * @group Local Position Estimator + * @unit m + * @min 1.0 + * @max 5.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f); + +/** + * Max EPV allowed for GPS initialization + * + * @group Local Position Estimator + * @unit m + * @min 1.0 + * @max 5.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f); + +/** + * Vision delay compensaton. + * + * Set to zero to enable automatic compensation from measurement timestamps + * + * @group Local Position Estimator + * @unit s + * @min 0 + * @max 0.1 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_DELAY, 0.1f); + +/** + * Vision xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.1f); + +/** + * Vision z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 100 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); + +/** + * Vicon position standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.0001 + * @max 1 + * @decimal 4 + */ +PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.001f); + +/** + * Position propagation noise density + * + * Increase to trust measurements more. + * Decrease to trust model more. + * + * @group Local Position Estimator + * @unit m/s/sqrt(Hz) + * @min 0 + * @max 1 + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f); + +/** + * Velocity propagation noise density + * + * Increase to trust measurements more. + * Decrease to trust model more. + * + * @group Local Position Estimator + * @unit m/s^2/sqrt(Hz) + * @min 0 + * @max 1 + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f); + +/** + * Accel bias propagation noise density + * + * @group Local Position Estimator + * @unit m/s^3/sqrt(Hz) + * @min 0 + * @max 1 + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f); + +/** + * Terrain random walk noise density, hilly/outdoor (0.1), flat/Indoor (0.001) + * + * @group Local Position Estimator + * @unit m/s/sqrt(Hz) + * @min 0 + * @max 1 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_PN_T, 0.001f); + +/** + * Terrain maximum percent grade, hilly/outdoor (100 = 45 deg), flat/Indoor (0 = 0 deg) + * + * Used to calculate increased terrain random walk nosie due to movement. + * + * @group Local Position Estimator + * @unit % + * @min 0 + * @max 100 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f); + +/** + * Flow gyro high pass filter cut off frequency + * + * @group Local Position Estimator + * @unit Hz + * @min 0 + * @max 2 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f); + +/** + * Enable publishing of a fake global position (e.g for AUTO missions using Optical Flow) + * + * By initializing the estimator to the LPE_LAT/LON parameters when global information is unavailable + * + * @group Local Position Estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(LPE_FAKE_ORIGIN, 0); + +/** + * Local origin latitude for nav w/o GPS + * + * @group Local Position Estimator + * @unit deg + * @min -90 + * @max 90 + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(LPE_LAT, 47.397742f); + +/** + * Local origin longitude for nav w/o GPS + * + * @group Local Position Estimator + * @unit deg + * @min -180 + * @max 180 + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(LPE_LON, 8.545594); + +/** + * Cut frequency for state publication + * + * @group Local Position Estimator + * @unit Hz + * @min 5 + * @max 1000 + * @decimal 0 + */ +PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f); + +/** + * Required velocity xy standard deviation to publish position + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 1.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.3f); + +/** + * Required z standard deviation to publish altitude/ terrain + * + * @group Local Position Estimator + * @unit m + * @min 0.3 + * @max 5.0 + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f); + +/** + * Land detector z standard deviation + * + * @group Local Position Estimator + * @unit m + * @min 0.001 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_LAND_Z, 0.03f); + +/** + * Land detector xy velocity standard deviation + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 10.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f); + +/** + * Minimum landing target standard covariance, uses reported covariance if greater. + * + * @group Local Position Estimator + * @unit m^2 + * @min 0.0 + * @max 10 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_LT_COV, 0.0001f); + +/** + * Integer bitmask controlling data fusion + * + * Set bits in the following positions to enable: + * 0 : Set to true to fuse GPS data if available, also requires GPS for altitude init + * 1 : Set to true to fuse optical flow data if available + * 2 : Set to true to fuse vision position + * 3 : Set to true to enable landing target + * 4 : Set to true to fuse land detector + * 5 : Set to true to publish AGL as local position down component + * 6 : Set to true to enable flow gyro compensation + * 7 : Set to true to enable baro fusion + * + * default (145 - GPS, baro, land detector) + * + * @group Local Position Estimator + * @min 0 + * @max 255 + * @bit 0 fuse GPS, requires GPS for alt. init + * @bit 1 fuse optical flow + * @bit 2 fuse vision position + * @bit 3 fuse landing target + * @bit 4 fuse land detector + * @bit 5 pub agl as lpos down + * @bit 6 flow gyro compensation + * @bit 7 fuse baro + */ +PARAM_DEFINE_INT32(LPE_FUSION, 145); diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/Flow+Noise+Modelling.pdf b/src/prometheus_px4/src/modules/local_position_estimator/sensors/Flow+Noise+Modelling.pdf new file mode 100644 index 0000000..fb5c035 Binary files /dev/null and b/src/prometheus_px4/src/modules/local_position_estimator/sensors/Flow+Noise+Modelling.pdf differ diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/baro.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/baro.cpp new file mode 100644 index 0000000..ea61f5c --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/baro.cpp @@ -0,0 +1,105 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_BARO_INIT_COUNT = 100; +static const uint32_t BARO_TIMEOUT = 100000; // 0.1 s + +void BlockLocalPositionEstimator::baroInit() +{ + // measure + Vector y; + + if (baroMeasure(y) != OK) { + _baroStats.reset(); + return; + } + + // if finished + if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { + _baroAltOrigin = _baroStats.getMean()(0); + mavlink_log_info(&mavlink_log_pub, + "[lpe] baro init %d m std %d cm", + (int)_baroStats.getMean()(0), + (int)(100 * _baroStats.getStdDev()(0))); + _sensorTimeout &= ~SENSOR_BARO; + _sensorFault &= ~SENSOR_BARO; + + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOriginGlobal = false; + _altOrigin = _baroAltOrigin; + } + } +} + +int BlockLocalPositionEstimator::baroMeasure(Vector &y) +{ + //measure + y.setZero(); + y(0) = _sub_airdata.get().baro_alt_meter; + _baroStats.update(y); + _time_last_baro = _sub_airdata.get().timestamp; + return OK; +} + +void BlockLocalPositionEstimator::baroCorrect() +{ + // measure + Vector y; + + if (baroMeasure(y) != OK) { return; } + + // subtract baro origin alt + y -= _baroAltOrigin; + + // baro measurement matrix + Matrix C; + C.setZero(); + C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. + + Matrix R; + R.setZero(); + R(0, 0) = _param_lpe_bar_z.get() * _param_lpe_bar_z.get(); + + // residual + Matrix S_I = + inv((C * m_P * C.transpose()) + R); + Vector r = y - (C * _x); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_baro]) { + if (!(_sensorFault & SENSOR_BARO)) { + mavlink_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", + double(r(0)), double(beta)); + _sensorFault |= SENSOR_BARO; + } + + } else if (_sensorFault & SENSOR_BARO) { + _sensorFault &= ~SENSOR_BARO; + mavlink_log_info(&mavlink_log_pub, "[lpe] baro OK"); + } + + // kalman filter correction always + Matrix K = m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; +} + +void BlockLocalPositionEstimator::baroCheckTimeout() +{ + if (_timeStamp - _time_last_baro > BARO_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_BARO)) { + _sensorTimeout |= SENSOR_BARO; + _baroStats.reset(); + mavlink_log_info(&mavlink_log_pub, "[lpe] baro timeout "); + } + } +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/flow.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/flow.cpp new file mode 100644 index 0000000..ff32dc5 --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/flow.cpp @@ -0,0 +1,215 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +// mavlink pub +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_FLOW_INIT_COUNT = 10; +static const uint32_t FLOW_TIMEOUT = 1000000; // 1 s + +void BlockLocalPositionEstimator::flowInit() +{ + // measure + Vector y; + + if (flowMeasure(y) != OK) { + _flowQStats.reset(); + return; + } + + // if finished + if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) { + mavlink_log_info(&mavlink_log_pub, "[lpe] flow init: " + "quality %d std %d", + int(_flowQStats.getMean()(0)), + int(_flowQStats.getStdDev()(0))); + _sensorTimeout &= ~SENSOR_FLOW; + _sensorFault &= ~SENSOR_FLOW; + } +} + +int BlockLocalPositionEstimator::flowMeasure(Vector &y) +{ + matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); + + // check for sane pitch/roll + if (euler.phi() > 0.5f || euler.theta() > 0.5f) { + return -1; + } + + // check for agl + if (agl() < _sub_flow.get().min_ground_distance) { + return -1; + } + + // check quality + float qual = _sub_flow.get().quality; + + if (qual < _param_lpe_flw_qmin.get()) { + return -1; + } + + // calculate range to center of image for flow + if (!(_estimatorInitialized & EST_TZ)) { + return -1; + } + + float d = agl() * cosf(euler.phi()) * cosf(euler.theta()); + + // optical flow in x, y axis + // TODO consider making flow scale a states of the kalman filter + float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _param_lpe_flw_scale.get(); + float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _param_lpe_flw_scale.get(); + float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f; + + if (dt_flow > 0.5f || dt_flow < 1.0e-6f) { + return -1; + } + + // angular rotation in x, y axis + float gyro_x_rad = 0; + float gyro_y_rad = 0; + + if (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) { + gyro_x_rad = _flow_gyro_x_high_pass.update( + _sub_flow.get().gyro_x_rate_integral); + gyro_y_rad = _flow_gyro_y_high_pass.update( + _sub_flow.get().gyro_y_rate_integral); + } + + //warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f", + //double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d)); + + // compute velocities in body frame using ground distance + // note that the integral rates in the optical_flow uORB topic are RH rotations about body axes + Vector3f delta_b( + +(flow_y_rad - gyro_y_rad) * d, + -(flow_x_rad - gyro_x_rad) * d, + 0); + + // rotation of flow from body to nav frame + Vector3f delta_n = _R_att * delta_b; + + // imporant to timestamp flow even if distance is bad + _time_last_flow = _timeStamp; + + // measurement + y(Y_flow_vx) = delta_n(0) / dt_flow; + y(Y_flow_vy) = delta_n(1) / dt_flow; + + _flowQStats.update(Scalarf(_sub_flow.get().quality)); + + return OK; +} + +void BlockLocalPositionEstimator::flowCorrect() +{ + // measure flow + Vector y; + + if (flowMeasure(y) != OK) { return; } + + // flow measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_flow_vx, X_vx) = 1; + C(Y_flow_vy, X_vy) = 1; + + SquareMatrix R; + R.setZero(); + + // polynomial noise model, found using least squares fit + // h, h**2, v, v*h, v*h**2 + const float p[5] = {0.04005232f, -0.00656446f, -0.26265873f, 0.13686658f, -0.00397357f}; + + // prevent extrapolation past end of polynomial fit by bounding independent variables + float h = agl(); + float v = y.norm(); + const float h_min = 2.0f; + const float h_max = 8.0f; + const float v_min = 0.5f; + const float v_max = 1.0f; + + if (h > h_max) { + h = h_max; + } + + if (h < h_min) { + h = h_min; + } + + if (v > v_max) { + v = v_max; + } + + if (v < v_min) { + v = v_min; + } + + // compute polynomial value + float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h; + + const Vector3f rates{_sub_angular_velocity.get().xyz}; + float rotrate_sq = rates(0) * rates(0) + + rates(1) * rates(1) + + rates(2) * rates(2); + + matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); + float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta(); + + R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev + + _param_lpe_flw_r.get() * _param_lpe_flw_r.get() * rot_sq + + _param_lpe_flw_rr.get() * _param_lpe_flw_rr.get() * rotrate_sq; + R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx); + + // residual + Vector r = y - C * _x; + + // residual covariance + Matrix S = C * m_P * C.transpose() + R; + + // publish innovations + _pub_innov.get().flow[0] = r(0); + _pub_innov.get().flow[1] = r(1); + _pub_innov_var.get().flow[0] = S(0, 0); + _pub_innov_var.get().flow[1] = S(1, 1); + + // residual covariance, (inverse) + Matrix S_I = inv(S); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_flow]) { + if (!(_sensorFault & SENSOR_FLOW)) { + mavlink_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta)); + _sensorFault |= SENSOR_FLOW; + } + + } else if (_sensorFault & SENSOR_FLOW) { + _sensorFault &= ~SENSOR_FLOW; + mavlink_log_info(&mavlink_log_pub, "[lpe] flow OK"); + } + + if (!(_sensorFault & SENSOR_FLOW)) { + Matrix K = + m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; + } +} + +void BlockLocalPositionEstimator::flowCheckTimeout() +{ + if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_FLOW)) { + _sensorTimeout |= SENSOR_FLOW; + _flowQStats.reset(); + mavlink_log_critical(&mavlink_log_pub, "[lpe] flow timeout "); + } + } +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/gps.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/gps.cpp new file mode 100644 index 0000000..3ef5ffa --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/gps.cpp @@ -0,0 +1,245 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_GPS_INIT_COUNT = 10; +static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s + +void BlockLocalPositionEstimator::gpsInit() +{ + // check for good gps signal + uint8_t nSat = _sub_gps.get().satellites_used; + float eph = _sub_gps.get().eph; + float epv = _sub_gps.get().epv; + uint8_t fix_type = _sub_gps.get().fix_type; + + if ( + nSat < 6 || + eph > _param_lpe_eph_max.get() || + epv > _param_lpe_epv_max.get() || + fix_type < 3 + ) { + _gpsStats.reset(); + return; + } + + // measure + Vector y; + + if (gpsMeasure(y) != OK) { + _gpsStats.reset(); + return; + } + + // if finished + if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) { + // get mean gps values + double gpsLat = _gpsStats.getMean()(0); + double gpsLon = _gpsStats.getMean()(1); + float gpsAlt = _gpsStats.getMean()(2); + + _sensorTimeout &= ~SENSOR_GPS; + _sensorFault &= ~SENSOR_GPS; + _gpsStats.reset(); + + if (!_receivedGps) { + // this is the first time we have received gps + _receivedGps = true; + + // note we subtract X_z which is in down directon so it is + // an addition + _gpsAltOrigin = gpsAlt + _x(X_z); + + // find lat, lon of current origin by subtracting x and y + // if not using vision position since vision will + // have it's own origin, not necessarily where vehicle starts + if (!_map_ref.init_done) { + double gpsLatOrigin = 0; + double gpsLonOrigin = 0; + // reproject at current coordinates + map_projection_init(&_map_ref, gpsLat, gpsLon); + // find origin + map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin); + // reinit origin + map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin); + // set timestamp when origin was set to current time + _time_origin = _timeStamp; + + // always override alt origin on first GPS to fix + // possible baro offset in global altitude at init + _altOrigin = _gpsAltOrigin; + _altOriginInitialized = true; + _altOriginGlobal = true; + + mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m", + gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin)); + } + + PX4_INFO("[lpe] gps init " + "lat %6.2f lon %6.2f alt %5.1f m", + gpsLat, + gpsLon, + double(gpsAlt)); + } + } +} + +int BlockLocalPositionEstimator::gpsMeasure(Vector &y) +{ + // gps measurement + y.setZero(); + y(0) = _sub_gps.get().lat * 1e-7; + y(1) = _sub_gps.get().lon * 1e-7; + y(2) = _sub_gps.get().alt * 1e-3; + y(3) = (double)_sub_gps.get().vel_n_m_s; + y(4) = (double)_sub_gps.get().vel_e_m_s; + y(5) = (double)_sub_gps.get().vel_d_m_s; + + // increament sums for mean + _gpsStats.update(y); + _time_last_gps = _timeStamp; + return OK; +} + +void BlockLocalPositionEstimator::gpsCorrect() +{ + // measure + Vector y_global; + + if (gpsMeasure(y_global) != OK) { return; } + + // gps measurement in local frame + double lat = y_global(Y_gps_x); + double lon = y_global(Y_gps_y); + float alt = y_global(Y_gps_z); + float px = 0; + float py = 0; + float pz = -(alt - _gpsAltOrigin); + map_projection_project(&_map_ref, lat, lon, &px, &py); + Vector y; + y.setZero(); + y(Y_gps_x) = px; + y(Y_gps_y) = py; + y(Y_gps_z) = pz; + y(Y_gps_vx) = y_global(Y_gps_vx); + y(Y_gps_vy) = y_global(Y_gps_vy); + y(Y_gps_vz) = y_global(Y_gps_vz); + + // gps measurement matrix, measures position and velocity + Matrix C; + C.setZero(); + C(Y_gps_x, X_x) = 1; + C(Y_gps_y, X_y) = 1; + C(Y_gps_z, X_z) = 1; + C(Y_gps_vx, X_vx) = 1; + C(Y_gps_vy, X_vy) = 1; + C(Y_gps_vz, X_vz) = 1; + + // gps covariance matrix + SquareMatrix R; + R.setZero(); + + // default to parameter, use gps cov if provided + float var_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get(); + float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get(); + float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get(); + float var_vz = _param_lpe_gps_vz.get() * _param_lpe_gps_vz.get(); + + // if field is not below minimum, set it to the value provided + if (_sub_gps.get().eph > _param_lpe_gps_xy.get()) { + var_xy = _sub_gps.get().eph * _sub_gps.get().eph; + } + + if (_sub_gps.get().epv > _param_lpe_gps_z.get()) { + var_z = _sub_gps.get().epv * _sub_gps.get().epv; + } + + float gps_s_stddev = _sub_gps.get().s_variance_m_s; + + if (gps_s_stddev > _param_lpe_gps_vxy.get()) { + var_vxy = gps_s_stddev * gps_s_stddev; + } + + if (gps_s_stddev > _param_lpe_gps_vz.get()) { + var_vz = gps_s_stddev * gps_s_stddev; + } + + R(0, 0) = var_xy; + R(1, 1) = var_xy; + R(2, 2) = var_z; + R(3, 3) = var_vxy; + R(4, 4) = var_vxy; + R(5, 5) = var_vz; + + // get delayed x + uint8_t i_hist = 0; + + if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; } + + Vector x0 = _xDelay.get(i_hist); + + // residual + Vector r = y - C * x0; + + // residual covariance + Matrix S = C * m_P * C.transpose() + R; + + // publish innovations + _pub_innov.get().gps_hpos[0] = r(0); + _pub_innov.get().gps_hpos[1] = r(1); + _pub_innov.get().gps_vpos = r(2); + _pub_innov.get().gps_hvel[0] = r(3); + _pub_innov.get().gps_hvel[1] = r(4); + _pub_innov.get().gps_vvel = r(5); + + // publish innovation variances + _pub_innov_var.get().gps_hpos[0] = S(0, 0); + _pub_innov_var.get().gps_hpos[1] = S(1, 1); + _pub_innov_var.get().gps_vpos = S(2, 2); + _pub_innov_var.get().gps_hvel[0] = S(3, 3); + _pub_innov_var.get().gps_hvel[1] = S(4, 4); + _pub_innov_var.get().gps_vvel = S(5, 5); + + // residual covariance, (inverse) + Matrix S_I = inv(S); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + // artifically increase beta threshhold to prevent fault during landing + float beta_thresh = 1e2f; + + if (beta / BETA_TABLE[n_y_gps] > beta_thresh) { + if (!(_sensorFault & SENSOR_GPS)) { + mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", + double(r(0) * r(0) / S_I(0, 0)), double(r(1) * r(1) / S_I(1, 1)), double(r(2) * r(2) / S_I(2, 2)), + double(r(3) * r(3) / S_I(3, 3)), double(r(4) * r(4) / S_I(4, 4)), double(r(5) * r(5) / S_I(5, 5))); + _sensorFault |= SENSOR_GPS; + } + + } else if (_sensorFault & SENSOR_GPS) { + _sensorFault &= ~SENSOR_GPS; + mavlink_log_info(&mavlink_log_pub, "[lpe] GPS OK"); + } + + // kalman filter correction always for GPS + Matrix K = m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; +} + +void BlockLocalPositionEstimator::gpsCheckTimeout() +{ + if (_timeStamp - _time_last_gps > GPS_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_GPS)) { + _sensorTimeout |= SENSOR_GPS; + _gpsStats.reset(); + mavlink_log_critical(&mavlink_log_pub, "[lpe] GPS timeout "); + } + } +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/land.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/land.cpp new file mode 100644 index 0000000..abb2fb3 --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/land.cpp @@ -0,0 +1,103 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +// +static const uint32_t REQ_LAND_INIT_COUNT = 1; +static const uint32_t LAND_TIMEOUT = 1000000; // 1.0 s + +void BlockLocalPositionEstimator::landInit() +{ + // measure + Vector y; + + if (landMeasure(y) != OK) { + _landCount = 0; + } + + // if finished + if (_landCount > REQ_LAND_INIT_COUNT) { + mavlink_log_info(&mavlink_log_pub, "[lpe] land init"); + _sensorTimeout &= ~SENSOR_LAND; + _sensorFault &= ~SENSOR_LAND; + } +} + +int BlockLocalPositionEstimator::landMeasure(Vector &y) +{ + _time_last_land = _timeStamp; + y.setZero(); + _landCount += 1; + return OK; +} + +void BlockLocalPositionEstimator::landCorrect() +{ + // measure land + Vector y; + + if (landMeasure(y) != OK) { return; } + + // measurement matrix + Matrix C; + C.setZero(); + // y = -(z - tz) + C(Y_land_vx, X_vx) = 1; + C(Y_land_vy, X_vy) = 1; + C(Y_land_agl, X_z) = -1;// measured altitude, negative down dir. + C(Y_land_agl, X_tz) = 1;// measured altitude, negative down dir. + + // use parameter covariance + SquareMatrix R; + R.setZero(); + R(Y_land_vx, Y_land_vx) = _param_lpe_land_vxy.get() * _param_lpe_land_vxy.get(); + R(Y_land_vy, Y_land_vy) = _param_lpe_land_vxy.get() * _param_lpe_land_vxy.get(); + R(Y_land_agl, Y_land_agl) = _param_lpe_land_z.get() * _param_lpe_land_z.get(); + + // residual + Matrix S_I = inv((C * m_P * C.transpose()) + R); + Vector r = y - C * _x; + _pub_innov.get().hagl = r(Y_land_agl); + _pub_innov_var.get().hagl = R(Y_land_agl, Y_land_agl); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + // artifically increase beta threshhold to prevent fault during landing + float beta_thresh = 1e2f; + + if (beta / BETA_TABLE[n_y_land] > beta_thresh) { + if (!(_sensorFault & SENSOR_LAND)) { + _sensorFault |= SENSOR_LAND; + mavlink_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta)); + } + + // abort correction + return; + + } else if (_sensorFault & SENSOR_LAND) { + _sensorFault &= ~SENSOR_LAND; + mavlink_log_info(&mavlink_log_pub, "[lpe] land OK"); + } + + // kalman filter correction always for land detector + Matrix K = m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; +} + +void BlockLocalPositionEstimator::landCheckTimeout() +{ + if (_timeStamp - _time_last_land > LAND_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_LAND)) { + _sensorTimeout |= SENSOR_LAND; + _landCount = 0; + mavlink_log_info(&mavlink_log_pub, "[lpe] land timeout "); + } + } +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/landing_target.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/landing_target.cpp new file mode 100644 index 0000000..6472fc1 --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/landing_target.cpp @@ -0,0 +1,121 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +static const uint64_t TARGET_TIMEOUT = 2000000; // [us] + +void BlockLocalPositionEstimator::landingTargetInit() +{ + if (_param_ltest_mode.get() == Target_Moving) { + // target is in moving mode, do not initialize + return; + } + + Vector y; + + if (landingTargetMeasure(y) == OK) { + mavlink_log_info(&mavlink_log_pub, "Landing target init"); + _sensorTimeout &= ~SENSOR_LAND_TARGET; + _sensorFault &= ~SENSOR_LAND_TARGET; + } +} + +int BlockLocalPositionEstimator::landingTargetMeasure(Vector &y) +{ + if (_param_ltest_mode.get() == Target_Stationary) { + if (_sub_landing_target_pose.get().rel_vel_valid) { + y(0) = _sub_landing_target_pose.get().vx_rel; + y(1) = _sub_landing_target_pose.get().vy_rel; + _time_last_target = _timeStamp; + + } else { + return -1; + } + + return OK; + + } + + return -1; +} + +void BlockLocalPositionEstimator::landingTargetCorrect() +{ + if (_param_ltest_mode.get() == Target_Moving) { + // nothing to do in this mode + return; + } + + // measure + Vector y; + + if (landingTargetMeasure(y) != OK) { return; } + + // calculate covariance + float cov_vx = _sub_landing_target_pose.get().cov_vx_rel; + float cov_vy = _sub_landing_target_pose.get().cov_vy_rel; + + // use sensor value only if reasoanble + if (cov_vx < _param_lpe_lt_cov.get() || cov_vy < _param_lpe_lt_cov.get()) { + cov_vx = _param_lpe_lt_cov.get(); + cov_vy = _param_lpe_lt_cov.get(); + } + + // target measurement matrix and noise matrix + Matrix C; + C.setZero(); + // residual = (y + vehicle velocity) + // sign change because target velocitiy is -vehicle velocity + C(Y_target_x, X_vx) = -1; + C(Y_target_y, X_vy) = -1; + + // covariance matrix + SquareMatrix R; + R.setZero(); + R(0, 0) = cov_vx; + R(1, 1) = cov_vy; + + // residual + Vector r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + inv(C * m_P * C.transpose() + R); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_target]) { + if (!(_sensorFault & SENSOR_LAND_TARGET)) { + mavlink_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta)); + _sensorFault |= SENSOR_LAND_TARGET; + } + + // abort correction + return; + + } else if (_sensorFault & SENSOR_LAND_TARGET) { + _sensorFault &= ~SENSOR_LAND_TARGET; + mavlink_log_info(&mavlink_log_pub, "Landing target OK"); + } + + // kalman filter correction + Matrix K = + m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; + +} + +void BlockLocalPositionEstimator::landingTargetCheckTimeout() +{ + if (_timeStamp - _time_last_target > TARGET_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_LAND_TARGET)) { + _sensorTimeout |= SENSOR_LAND_TARGET; + mavlink_log_info(&mavlink_log_pub, "Landing target timeout"); + } + } +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/lidar.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/lidar.cpp new file mode 100644 index 0000000..515b8f5 --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/lidar.cpp @@ -0,0 +1,134 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +// +static const uint32_t REQ_LIDAR_INIT_COUNT = 10; +static const uint32_t LIDAR_TIMEOUT = 1000000; // 1.0 s + +void BlockLocalPositionEstimator::lidarInit() +{ + // measure + Vector y; + + if (lidarMeasure(y) != OK) { + _lidarStats.reset(); + } + + // if finished + if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) { + mavlink_log_info(&mavlink_log_pub, "[lpe] lidar init: " + "mean %d cm stddev %d cm", + int(100 * _lidarStats.getMean()(0)), + int(100 * _lidarStats.getStdDev()(0))); + _sensorTimeout &= ~SENSOR_LIDAR; + _sensorFault &= ~SENSOR_LIDAR; + } +} + +int BlockLocalPositionEstimator::lidarMeasure(Vector &y) +{ + // measure + float d = _sub_lidar->get().current_distance; + float eps = 0.01f; // 1 cm + float min_dist = _sub_lidar->get().min_distance + eps; + float max_dist = _sub_lidar->get().max_distance - eps; + + // prevent driver from setting min dist below eps + if (min_dist < eps) { + min_dist = eps; + } + + // check for bad data + if (d > max_dist || d < min_dist) { + return -1; + } + + // update stats + _lidarStats.update(Scalarf(d)); + _time_last_lidar = _timeStamp; + y.setZero(); + matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); + y(0) = (d + _param_lpe_ldr_off_z.get()) * + cosf(euler.phi()) * + cosf(euler.theta()); + return OK; +} + +void BlockLocalPositionEstimator::lidarCorrect() +{ + // measure lidar + Vector y; + + if (lidarMeasure(y) != OK) { return; } + + // measurement matrix + Matrix C; + C.setZero(); + // y = -(z - tz) + // TODO could add trig to make this an EKF correction + C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir. + C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir. + + // use parameter covariance unless sensor provides reasonable value + SquareMatrix R; + R.setZero(); + float cov = _sub_lidar->get().variance; + + if (cov < 1.0e-3f) { + R(0, 0) = _param_lpe_ldr_z.get() * _param_lpe_ldr_z.get(); + + } else { + R(0, 0) = cov; + } + + // residual + Vector r = y - C * _x; + // residual covariance + Matrix S = C * m_P * C.transpose() + R; + + // publish innovations + _pub_innov.get().hagl = r(0); + _pub_innov_var.get().hagl = S(0, 0); + + // residual covariance, (inverse) + Matrix S_I = inv(S); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_lidar]) { + if (!(_sensorFault & SENSOR_LIDAR)) { + mavlink_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta)); + _sensorFault |= SENSOR_LIDAR; + } + + // abort correction + return; + + } else if (_sensorFault & SENSOR_LIDAR) { + _sensorFault &= ~SENSOR_LIDAR; + mavlink_log_info(&mavlink_log_pub, "[lpe] lidar OK"); + } + + // kalman filter correction always + Matrix K = m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; +} + +void BlockLocalPositionEstimator::lidarCheckTimeout() +{ + if (_timeStamp - _time_last_lidar > LIDAR_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_LIDAR)) { + _sensorTimeout |= SENSOR_LIDAR; + _lidarStats.reset(); + mavlink_log_info(&mavlink_log_pub, "[lpe] lidar timeout "); + } + } +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/mocap.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/mocap.cpp new file mode 100644 index 0000000..c62098a --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/mocap.cpp @@ -0,0 +1,198 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const uint32_t REQ_MOCAP_INIT_COUNT = 20; +static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s + +// set pose/velocity as invalid if standard deviation is bigger than EP_MAX_STD_DEV +// TODO: the user should be allowed to set these values by a parameter +static constexpr float EP_MAX_STD_DEV = 100.0f; + +void BlockLocalPositionEstimator::mocapInit() +{ + // measure + Vector y; + + if (mocapMeasure(y) != OK) { + _mocapStats.reset(); + return; + } + + // if finished + if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { + mavlink_log_info(&mavlink_log_pub, "[lpe] mocap position init: " + "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m", + double(_mocapStats.getMean()(0)), + double(_mocapStats.getMean()(1)), + double(_mocapStats.getMean()(2)), + double(_mocapStats.getStdDev()(0)), + double(_mocapStats.getStdDev()(1)), + double(_mocapStats.getStdDev()(2))); + _sensorTimeout &= ~SENSOR_MOCAP; + _sensorFault &= ~SENSOR_MOCAP; + + // get reference for global position + _ref_lat = math::degrees(_global_local_proj_ref.lat_rad); + _ref_lon = math::degrees(_global_local_proj_ref.lon_rad); + _ref_alt = _global_local_alt0; + + _is_global_cov_init = map_projection_initialized(&_global_local_proj_ref); + + if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) { + // initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well) + mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m", + double(_ref_lat), double(_ref_lon), double(_ref_alt)); + map_projection_init(&_map_ref, _ref_lat, _ref_lon); + // set timestamp when origin was set to current time + _time_origin = _timeStamp; + } + + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOriginGlobal = true; + _altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f; + } + } +} + +int BlockLocalPositionEstimator::mocapMeasure(Vector &y) +{ + uint8_t x_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_X_VARIANCE; + uint8_t y_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Y_VARIANCE; + uint8_t z_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Z_VARIANCE; + + if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[x_variance])) { + // check if the mocap data is valid based on the covariances + _mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[x_variance], + _sub_mocap_odom.get().pose_covariance[y_variance])); + _mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[z_variance]); + _mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV; + _mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV; + + } else { + // if we don't have covariances, assume every reading + _mocap_xy_valid = true; + _mocap_z_valid = true; + } + + if (!_mocap_xy_valid || !_mocap_z_valid) { + _time_last_mocap = _sub_mocap_odom.get().timestamp_sample; + return -1; + + } else { + _time_last_mocap = _sub_mocap_odom.get().timestamp_sample; + + if (PX4_ISFINITE(_sub_mocap_odom.get().x)) { + y.setZero(); + y(Y_mocap_x) = _sub_mocap_odom.get().x; + y(Y_mocap_y) = _sub_mocap_odom.get().y; + y(Y_mocap_z) = _sub_mocap_odom.get().z; + _mocapStats.update(y); + + return OK; + + } else { + return -1; + } + } +} + +void BlockLocalPositionEstimator::mocapCorrect() +{ + // measure + Vector y; + + if (mocapMeasure(y) != OK) { + mavlink_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph, + (double)_mocap_epv); + return; + } + + // mocap measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_mocap_x, X_x) = 1; + C(Y_mocap_y, X_y) = 1; + C(Y_mocap_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + + // use std dev from mocap data if available + if (_mocap_eph > _param_lpe_vic_p.get()) { + R(Y_mocap_x, Y_mocap_x) = _mocap_eph * _mocap_eph; + R(Y_mocap_y, Y_mocap_y) = _mocap_eph * _mocap_eph; + + } else { + R(Y_mocap_x, Y_mocap_x) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get(); + R(Y_mocap_y, Y_mocap_y) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get(); + } + + if (_mocap_epv > _param_lpe_vic_p.get()) { + R(Y_mocap_z, Y_mocap_z) = _mocap_epv * _mocap_epv; + + } else { + R(Y_mocap_z, Y_mocap_z) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get(); + } + + // residual + Vector r = y - C * _x; + // residual covariance + Matrix S = C * m_P * C.transpose() + R; + + // publish innovations + _pub_innov.get().ev_hpos[0] = r(0); + _pub_innov.get().ev_hpos[1] = r(1); + _pub_innov.get().ev_vpos = r(2); + _pub_innov.get().ev_hvel[0] = NAN; + _pub_innov.get().ev_hvel[1] = NAN; + _pub_innov.get().ev_vvel = NAN; + + // publish innovation variances + _pub_innov_var.get().ev_hpos[0] = S(0, 0); + _pub_innov_var.get().ev_hpos[1] = S(1, 1); + _pub_innov_var.get().ev_vpos = S(2, 2); + _pub_innov_var.get().ev_hvel[0] = NAN; + _pub_innov_var.get().ev_hvel[1] = NAN; + _pub_innov_var.get().ev_vvel = NAN; + + // residual covariance, (inverse) + Matrix S_I = inv(S); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_mocap]) { + if (!(_sensorFault & SENSOR_MOCAP)) { + //mavlink_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta)); + _sensorFault |= SENSOR_MOCAP; + } + + } else if (_sensorFault & SENSOR_MOCAP) { + _sensorFault &= ~SENSOR_MOCAP; + //mavlink_log_info(&mavlink_log_pub, "[lpe] mocap OK"); + } + + // kalman filter correction always + Matrix K = m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; +} + +void BlockLocalPositionEstimator::mocapCheckTimeout() +{ + if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_MOCAP)) { + _sensorTimeout |= SENSOR_MOCAP; + _mocapStats.reset(); + mavlink_log_info(&mavlink_log_pub, "[lpe] mocap timeout "); + } + } +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/sonar.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/sonar.cpp new file mode 100644 index 0000000..c39e3f1 --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/sonar.cpp @@ -0,0 +1,157 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor +// to initialize +static const int REQ_SONAR_INIT_COUNT = 10; +static const uint32_t SONAR_TIMEOUT = 5000000; // 2.0 s +static const float SONAR_MAX_INIT_STD = 0.3f; // meters + +void BlockLocalPositionEstimator::sonarInit() +{ + // measure + Vector y; + + if (_sonarStats.getCount() == 0) { + _time_init_sonar = _timeStamp; + } + + if (sonarMeasure(y) != OK) { + return; + } + + // if finished + if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) { + if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) { + mavlink_log_info(&mavlink_log_pub, "[lpe] sonar init std > min"); + _sonarStats.reset(); + + } else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) { + mavlink_log_info(&mavlink_log_pub, "[lpe] sonar init timeout "); + _sonarStats.reset(); + + } else { + PX4_INFO("[lpe] sonar init " + "mean %d cm std %d cm", + int(100 * _sonarStats.getMean()(0)), + int(100 * _sonarStats.getStdDev()(0))); + _sensorTimeout &= ~SENSOR_SONAR; + _sensorFault &= ~SENSOR_SONAR; + } + } +} + +int BlockLocalPositionEstimator::sonarMeasure(Vector &y) +{ + // measure + float d = _sub_sonar->get().current_distance; + float eps = 0.01f; // 1 cm + float min_dist = _sub_sonar->get().min_distance + eps; + float max_dist = _sub_sonar->get().max_distance - eps; + + // prevent driver from setting min dist below eps + if (min_dist < eps) { + min_dist = eps; + } + + // check for bad data + if (d > max_dist || d < min_dist) { + return -1; + } + + // update stats + _sonarStats.update(Scalarf(d)); + _time_last_sonar = _timeStamp; + y.setZero(); + matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); + y(0) = (d + _param_lpe_snr_off_z.get()) * + cosf(euler.phi()) * + cosf(euler.theta()); + return OK; +} + +void BlockLocalPositionEstimator::sonarCorrect() +{ + // measure + Vector y; + + if (sonarMeasure(y) != OK) { return; } + + // do not use sonar if lidar is active and not faulty or timed out + if (_lidarUpdated + && !(_sensorFault & SENSOR_LIDAR) + && !(_sensorTimeout & SENSOR_LIDAR)) { return; } + + // calculate covariance + float cov = _sub_sonar->get().variance; + + if (cov < 1.0e-3f) { + // use sensor value if reasoanble + cov = _param_lpe_snr_z.get() * _param_lpe_snr_z.get(); + } + + // sonar measurement matrix and noise matrix + Matrix C; + C.setZero(); + // y = -(z - tz) + // TODO could add trig to make this an EKF correction + C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir. + C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir. + + // covariance matrix + SquareMatrix R; + R.setZero(); + R(0, 0) = cov; + + // residual + Vector r = y - C * _x; + // residual covariance + Matrix S = C * m_P * C.transpose() + R; + + // publish innovations + _pub_innov.get().hagl = r(0); + _pub_innov_var.get().hagl = S(0, 0); + + // residual covariance, (inverse) + Matrix S_I = inv(S); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_sonar]) { + if (!(_sensorFault & SENSOR_SONAR)) { + _sensorFault |= SENSOR_SONAR; + mavlink_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta)); + } + + // abort correction + return; + + } else if (_sensorFault & SENSOR_SONAR) { + _sensorFault &= ~SENSOR_SONAR; + //mavlink_log_info(&mavlink_log_pub, "[lpe] sonar OK"); + } + + // kalman filter correction if no fault + if (!(_sensorFault & SENSOR_SONAR)) { + Matrix K = + m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; + } +} + +void BlockLocalPositionEstimator::sonarCheckTimeout() +{ + if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_SONAR)) { + _sensorTimeout |= SENSOR_SONAR; + _sonarStats.reset(); + mavlink_log_info(&mavlink_log_pub, "[lpe] sonar timeout "); + } + } +} diff --git a/src/prometheus_px4/src/modules/local_position_estimator/sensors/vision.cpp b/src/prometheus_px4/src/modules/local_position_estimator/sensors/vision.cpp new file mode 100644 index 0000000..9c0dfff --- /dev/null +++ b/src/prometheus_px4/src/modules/local_position_estimator/sensors/vision.cpp @@ -0,0 +1,217 @@ +#include "../BlockLocalPositionEstimator.hpp" +#include +#include + +extern orb_advert_t mavlink_log_pub; + +// required number of samples for sensor to initialize. +// This is a vision based position measurement so we assume +// as soon as we get one measurement it is initialized. +static const uint32_t REQ_VISION_INIT_COUNT = 1; + +// We don't want to deinitialize it because +// this will throw away a correction before it starts using the data so we +// set the timeout to 0.5 seconds +static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s + +// set pose/velocity as invalid if standard deviation is bigger than EP_MAX_STD_DEV +// TODO: the user should be allowed to set these values by a parameter +static constexpr float EP_MAX_STD_DEV = 100.0f; + +void BlockLocalPositionEstimator::visionInit() +{ + // measure + Vector y; + + if (visionMeasure(y) != OK) { + _visionStats.reset(); + return; + } + + // increament sums for mean + if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { + mavlink_log_info(&mavlink_log_pub, "[lpe] vision position init: " + "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", + double(_visionStats.getMean()(0)), + double(_visionStats.getMean()(1)), + double(_visionStats.getMean()(2)), + double(_visionStats.getStdDev()(0)), + double(_visionStats.getStdDev()(1)), + double(_visionStats.getStdDev()(2))); + _sensorTimeout &= ~SENSOR_VISION; + _sensorFault &= ~SENSOR_VISION; + + // get reference for global position + _ref_lat = math::degrees(_global_local_proj_ref.lat_rad); + _ref_lon = math::degrees(_global_local_proj_ref.lon_rad); + _ref_alt = _global_local_alt0; + + _is_global_cov_init = map_projection_initialized(&_global_local_proj_ref); + + if (!_map_ref.init_done && _is_global_cov_init) { + // initialize global origin using the visual estimator reference + mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m", + double(_ref_lat), double(_ref_lon), double(_ref_alt)); + map_projection_init(&_map_ref, _ref_lat, _ref_lon); + // set timestamp when origin was set to current time + _time_origin = _timeStamp; + } + + if (!_altOriginInitialized) { + _altOriginInitialized = true; + _altOriginGlobal = true; + _altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f; + } + } +} + +int BlockLocalPositionEstimator::visionMeasure(Vector &y) +{ + uint8_t x_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_X_VARIANCE; + uint8_t y_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_Y_VARIANCE; + uint8_t z_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_Z_VARIANCE; + + if (PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[x_variance])) { + // check if the vision data is valid based on the covariances + _vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[x_variance], + _sub_visual_odom.get().pose_covariance[y_variance])); + _vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[z_variance]); + _vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV; + _vision_z_valid = _vision_epv <= EP_MAX_STD_DEV; + + } else { + // if we don't have covariances, assume every reading + _vision_xy_valid = true; + _vision_z_valid = true; + } + + if (!_vision_xy_valid || !_vision_z_valid) { + _time_last_vision_p = _sub_visual_odom.get().timestamp_sample; + return -1; + + } else { + _time_last_vision_p = _sub_visual_odom.get().timestamp_sample; + + if (PX4_ISFINITE(_sub_visual_odom.get().x)) { + y.setZero(); + y(Y_vision_x) = _sub_visual_odom.get().x; + y(Y_vision_y) = _sub_visual_odom.get().y; + y(Y_vision_z) = _sub_visual_odom.get().z; + _visionStats.update(y); + + return OK; + + } else { + return -1; + } + } +} + +void BlockLocalPositionEstimator::visionCorrect() +{ + // measure + Vector y; + + if (visionMeasure(y) != OK) { + mavlink_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph, + (double)_vision_epv); + return; + } + + // vision measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_vision_x, X_x) = 1; + C(Y_vision_y, X_y) = 1; + C(Y_vision_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + + // use std dev from vision data if available + if (_vision_eph > _param_lpe_vis_xy.get()) { + R(Y_vision_x, Y_vision_x) = _vision_eph * _vision_eph; + R(Y_vision_y, Y_vision_y) = _vision_eph * _vision_eph; + + } else { + R(Y_vision_x, Y_vision_x) = _param_lpe_vis_xy.get() * _param_lpe_vis_xy.get(); + R(Y_vision_y, Y_vision_y) = _param_lpe_vis_xy.get() * _param_lpe_vis_xy.get(); + } + + if (_vision_epv > _param_lpe_vis_z.get()) { + R(Y_vision_z, Y_vision_z) = _vision_epv * _vision_epv; + + } else { + R(Y_vision_z, Y_vision_z) = _param_lpe_vis_z.get() * _param_lpe_vis_z.get(); + } + + // vision delayed x + uint8_t i_hist = 0; + + float vision_delay = (_timeStamp - _sub_visual_odom.get().timestamp_sample) * 1e-6f; // measurement delay in seconds + + if (vision_delay < 0.0f) { vision_delay = 0.0f; } + + // use auto-calculated delay from measurement if parameter is set to zero + if (getDelayPeriods(_param_lpe_vis_delay.get() > 0.0f ? _param_lpe_vis_delay.get() : vision_delay, &i_hist) < 0) { return; } + + Vector x0 = _xDelay.get(i_hist); + + // residual + Matrix r = y - C * x0; + // residual covariance + Matrix S = C * m_P * C.transpose() + R; + + // publish innovations + _pub_innov.get().ev_hpos[0] = r(0, 0); + _pub_innov.get().ev_hpos[1] = r(1, 0); + _pub_innov.get().ev_vpos = r(2, 0); + _pub_innov.get().ev_hvel[0] = NAN; + _pub_innov.get().ev_hvel[1] = NAN; + _pub_innov.get().ev_vvel = NAN; + + // publish innovation variances + _pub_innov_var.get().ev_hpos[0] = S(0, 0); + _pub_innov_var.get().ev_hpos[1] = S(1, 1); + _pub_innov_var.get().ev_vpos = S(2, 2); + _pub_innov_var.get().ev_hvel[0] = NAN; + _pub_innov_var.get().ev_hvel[1] = NAN; + _pub_innov_var.get().ev_vvel = NAN; + + // residual covariance, (inverse) + Matrix S_I = inv(S); + + // fault detection + float beta = (r.transpose() * (S_I * r))(0, 0); + + if (beta > BETA_TABLE[n_y_vision]) { + if (!(_sensorFault & SENSOR_VISION)) { + mavlink_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta)); + _sensorFault |= SENSOR_VISION; + } + + } else if (_sensorFault & SENSOR_VISION) { + _sensorFault &= ~SENSOR_VISION; + mavlink_log_info(&mavlink_log_pub, "[lpe] vision position OK"); + } + + // kalman filter correction if no fault + if (!(_sensorFault & SENSOR_VISION)) { + Matrix K = m_P * C.transpose() * S_I; + Vector dx = K * r; + _x += dx; + m_P -= K * C * m_P; + } +} + +void BlockLocalPositionEstimator::visionCheckTimeout() +{ + if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) { + if (!(_sensorTimeout & SENSOR_VISION)) { + _sensorTimeout |= SENSOR_VISION; + _visionStats.reset(); + mavlink_log_critical(&mavlink_log_pub, "[lpe] vision position timeout "); + } + } +} diff --git a/src/prometheus_px4/src/modules/logger/CMakeLists.txt b/src/prometheus_px4/src/modules/logger/CMakeLists.txt new file mode 100644 index 0000000..9d60069 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +px4_add_module( + MODULE modules__logger + MAIN logger + PRIORITY "SCHED_PRIORITY_MAX-30" + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + logged_topics.cpp + logger.cpp + log_writer.cpp + log_writer_file.cpp + log_writer_mavlink.cpp + util.cpp + watchdog.cpp + DEPENDS + version + ) diff --git a/src/prometheus_px4/src/modules/logger/log_writer.cpp b/src/prometheus_px4/src/modules/logger/log_writer.cpp new file mode 100644 index 0000000..8511f9e --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/log_writer.cpp @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "log_writer.h" + +namespace px4 +{ +namespace logger +{ + +LogWriter::LogWriter(Backend configured_backend, size_t file_buffer_size) + : _backend(configured_backend) +{ + if (configured_backend & BackendFile) { + _log_writer_file_for_write = _log_writer_file = new LogWriterFile(file_buffer_size); + + if (!_log_writer_file) { + PX4_ERR("LogWriterFile allocation failed"); + } + } + + if (configured_backend & BackendMavlink) { + _log_writer_mavlink_for_write = _log_writer_mavlink = new LogWriterMavlink(); + + if (!_log_writer_mavlink) { + PX4_ERR("LogWriterMavlink allocation failed"); + } + } +} + +bool LogWriter::init() +{ + if (_log_writer_file) { + if (!_log_writer_file->init()) { + PX4_ERR("alloc failed"); + return false; + } + + int ret = _log_writer_file->thread_start(); + + if (ret) { + PX4_ERR("failed to create writer thread (%i)", ret); + return false; + } + } + + if (_log_writer_mavlink) { + if (!_log_writer_mavlink->init()) { + PX4_ERR("mavlink init failed"); + return false; + } + } + + return true; +} + +LogWriter::~LogWriter() +{ + if (_log_writer_file) { + delete (_log_writer_file); + } + + if (_log_writer_mavlink) { + delete (_log_writer_mavlink); + } +} + +bool LogWriter::is_started(LogType type) const +{ + bool ret = false; + + if (_log_writer_file) { + ret = _log_writer_file->is_started(type); + } + + if (_log_writer_mavlink && type == LogType::Full) { + ret = ret || _log_writer_mavlink->is_started(); + } + + return ret; +} + +bool LogWriter::is_started(LogType type, Backend query_backend) const +{ + if (query_backend == BackendFile && _log_writer_file) { + return _log_writer_file->is_started(type); + } + + if (query_backend == BackendMavlink && _log_writer_mavlink && type == LogType::Full) { + return _log_writer_mavlink->is_started(); + } + + return false; +} + +void LogWriter::start_log_file(LogType type, const char *filename) +{ + if (_log_writer_file) { + _log_writer_file->start_log(type, filename); + } +} + +void LogWriter::stop_log_file(LogType type) +{ + if (_log_writer_file) { + _log_writer_file->stop_log(type); + } +} + +void LogWriter::start_log_mavlink() +{ + if (_log_writer_mavlink) { + _log_writer_mavlink->start_log(); + } +} + +void LogWriter::stop_log_mavlink() +{ + if (_log_writer_mavlink) { + _log_writer_mavlink->stop_log(); + } +} + +void LogWriter::thread_stop() +{ + if (_log_writer_file) { + _log_writer_file->thread_stop(); + } +} + +int LogWriter::write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start) +{ + int ret_file = 0, ret_mavlink = 0; + + if (_log_writer_file_for_write) { + ret_file = _log_writer_file_for_write->write_message(type, ptr, size, dropout_start); + } + + if (_log_writer_mavlink_for_write && type == LogType::Full) { + ret_mavlink = _log_writer_mavlink_for_write->write_message(ptr, size); + } + + // file backend errors takes precedence + if (ret_file != 0) { + return ret_file; + } + + return ret_mavlink; +} + +void LogWriter::select_write_backend(Backend sel_backend) +{ + if (sel_backend & BackendFile) { + _log_writer_file_for_write = _log_writer_file; + + } else { + _log_writer_file_for_write = nullptr; + } + + if (sel_backend & BackendMavlink) { + _log_writer_mavlink_for_write = _log_writer_mavlink; + + } else { + _log_writer_mavlink_for_write = nullptr; + } +} + +} +} diff --git a/src/prometheus_px4/src/modules/logger/log_writer.h b/src/prometheus_px4/src/modules/logger/log_writer.h new file mode 100644 index 0000000..abac8a5 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/log_writer.h @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include "log_writer_file.h" +#include "log_writer_mavlink.h" + +namespace px4 +{ +namespace logger +{ + +/** + * @class LogWriter + * Manages starting, stopping & writing of logged data using the configured backend. + */ +class LogWriter +{ +public: + + /** bitfield to specify a backend */ + typedef uint8_t Backend; + static constexpr Backend BackendFile = 1 << 0; + static constexpr Backend BackendMavlink = 1 << 1; + static constexpr Backend BackendAll = BackendFile | BackendMavlink; + + LogWriter(Backend configured_backend, size_t file_buffer_size); + ~LogWriter(); + + bool init(); + + Backend backend() const { return _backend; } + + /** stop all running threads and wait for them to exit */ + void thread_stop(); + + void start_log_file(LogType type, const char *filename); + + void stop_log_file(LogType type); + + void start_log_mavlink(); + + void stop_log_mavlink(); + + /** + * whether logging is currently active or not (any of the selected backends). + */ + bool is_started(LogType type) const; + + /** + * whether logging is currently active or not for a specific backend. + */ + bool is_started(LogType type, Backend query_backend) const; + + /** + * Write a single ulog message (including header). The caller must call lock() before calling this. + * @param dropout_start timestamp when lastest dropout occured. 0 if no dropout at the moment. + * @return 0 on success (or if no logging started), + * -1 if not enough space in the buffer left (file backend), -2 mavlink backend failed + * add type -> pass through, but not to mavlink if mission log + */ + int write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start = 0); + + /** + * Select a backend, so that future calls to write_message() only write to the selected + * sel_backend, until unselect_write_backend() is called. + * @param backend + */ + void select_write_backend(Backend sel_backend); + void unselect_write_backend() { select_write_backend(BackendAll); } + + /* file logging methods */ + + void lock() + { + if (_log_writer_file) { _log_writer_file->lock(); } + } + + void unlock() + { + if (_log_writer_file) { _log_writer_file->unlock(); } + } + + void notify() + { + if (_log_writer_file) { _log_writer_file->notify(); } + } + + size_t get_total_written_file(LogType type) const + { + if (_log_writer_file) { return _log_writer_file->get_total_written(type); } + + return 0; + } + + size_t get_buffer_size_file(LogType type) const + { + if (_log_writer_file) { return _log_writer_file->get_buffer_size(type); } + + return 0; + } + + size_t get_buffer_fill_count_file(LogType type) const + { + if (_log_writer_file) { return _log_writer_file->get_buffer_fill_count(type); } + + return 0; + } + + pthread_t thread_id_file() const + { + if (_log_writer_file) { return _log_writer_file->thread_id(); } + + return (pthread_t)0; + } + + + /** + * Indicate to the underlying backend whether future write_message() calls need a reliable + * transfer. Needed for header integrity. + */ + void set_need_reliable_transfer(bool need_reliable) + { + if (_log_writer_file) { _log_writer_file->set_need_reliable_transfer(need_reliable); } + + if (_log_writer_mavlink) { _log_writer_mavlink->set_need_reliable_transfer(need_reliable); } + } + + bool need_reliable_transfer() const + { + if (_log_writer_file) { return _log_writer_file->need_reliable_transfer(); } + + if (_log_writer_mavlink) { return _log_writer_mavlink->need_reliable_transfer(); } + + return false; + } + +private: + + LogWriterFile *_log_writer_file = nullptr; + LogWriterMavlink *_log_writer_mavlink = nullptr; + + LogWriterFile *_log_writer_file_for_write = + nullptr; ///< pointer that is used for writing, to temporarily select write backends + LogWriterMavlink *_log_writer_mavlink_for_write = nullptr; + + const Backend _backend; +}; + + +} +} diff --git a/src/prometheus_px4/src/modules/logger/log_writer_file.cpp b/src/prometheus_px4/src/modules/logger/log_writer_file.cpp new file mode 100644 index 0000000..484df10 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/log_writer_file.cpp @@ -0,0 +1,519 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "log_writer_file.h" +#include "messages.h" + +#include +#include +#include + +#include +#include +#ifdef __PX4_NUTTX +#include +#endif /* __PX4_NUTTX */ + +using namespace time_literals; + + +namespace px4 +{ +namespace logger +{ +constexpr size_t LogWriterFile::_min_write_chunk; + +LogWriterFile::LogWriterFile(size_t buffer_size) + : _buffers{ + //We always write larger chunks (orb messages) to the buffer, so the buffer + //needs to be larger than the minimum write chunk (300 is somewhat arbitrary) + { + math::max(buffer_size, _min_write_chunk + 300), + perf_alloc(PC_ELAPSED, "logger_sd_write"), perf_alloc(PC_ELAPSED, "logger_sd_fsync")}, + + { + 300, // buffer size for the mission log (can be kept fairly small) + perf_alloc(PC_ELAPSED, "logger_sd_write_mission"), perf_alloc(PC_ELAPSED, "logger_sd_fsync_mission")} +} +{ + pthread_mutex_init(&_mtx, nullptr); + pthread_cond_init(&_cv, nullptr); +} + +bool LogWriterFile::init() +{ + return true; +} + +LogWriterFile::~LogWriterFile() +{ + pthread_mutex_destroy(&_mtx); + pthread_cond_destroy(&_cv); +} + +void LogWriterFile::start_log(LogType type, const char *filename) +{ + // At this point we don't expect the file to be open, but it can happen for very fast consecutive stop & start + // calls. In that case we wait for the thread to close the file first. + lock(); + + while (_buffers[(int)type].fd() >= 0) { + unlock(); + system_usleep(5000); + lock(); + } + + unlock(); + + if (type == LogType::Full) { + // register the current file with the hardfault handler: if the system crashes, + // the hardfault handler will append the crash log to that file on the next reboot. + // Note that we don't deregister it when closing the log, so that crashes after disarming + // are appended as well (the same holds for crashes before arming, which can be a bit misleading) + int ret = hardfault_store_filename(filename); + + if (ret) { + PX4_ERR("Failed to register ULog file to the hardfault handler (%i)", ret); + } + } + + if (_buffers[(int)type].start_log(filename)) { + PX4_INFO("Opened %s log file: %s", log_type_str(type), filename); + notify(); + } +} + +int LogWriterFile::hardfault_store_filename(const char *log_file) +{ +#ifdef __PX4_NUTTX + int fd = open(HARDFAULT_ULOG_PATH, O_TRUNC | O_WRONLY | O_CREAT); + + if (fd < 0) { + return -errno; + } + + int n = strlen(log_file); + + if (n >= HARDFAULT_MAX_ULOG_FILE_LEN) { + PX4_ERR("ULog file name too long (%s, %i>=%i)\n", log_file, n, HARDFAULT_MAX_ULOG_FILE_LEN); + return -EINVAL; + } + + if (n + 1 != ::write(fd, log_file, n + 1)) { + close(fd); + return -errno; + } + + int ret = close(fd); + + if (ret != 0) { + return -errno; + } + +#endif /* __PX4_NUTTX */ + + return 0; +} + +void LogWriterFile::stop_log(LogType type) +{ + _buffers[(int)type]._should_run = false; + notify(); +} + +int LogWriterFile::thread_start() +{ + pthread_attr_t thr_attr; + pthread_attr_init(&thr_attr); + + sched_param param; + /* low priority, as this is expensive disk I/O */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; + (void)pthread_attr_setschedparam(&thr_attr, ¶m); + + pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1170)); + + int ret = pthread_create(&_thread, &thr_attr, &LogWriterFile::run_helper, this); + pthread_attr_destroy(&thr_attr); + + return ret; +} + +void LogWriterFile::thread_stop() +{ + // this will terminate the main loop of the writer thread + _exit_thread = true; + _buffers[0]._should_run = _buffers[1]._should_run = false; + + notify(); + + // wait for thread to complete + int ret = pthread_join(_thread, nullptr); + + if (ret) { + PX4_WARN("join failed: %d", ret); + } + +} + +void *LogWriterFile::run_helper(void *context) +{ + px4_prctl(PR_SET_NAME, "log_writer_file", px4_getpid()); + + static_cast(context)->run(); + return nullptr; +} + +void LogWriterFile::run() +{ + while (!_exit_thread) { + // Outer endless loop + // Wait for _should_run flag + while (!_exit_thread) { + bool start = false; + pthread_mutex_lock(&_mtx); + pthread_cond_wait(&_cv, &_mtx); + start = _buffers[0]._should_run || _buffers[1]._should_run; + pthread_mutex_unlock(&_mtx); + + if (start) { + break; + } + } + + if (_exit_thread) { + break; + } + + int poll_count = 0; + int written = 0; + hrt_abstime last_fsync = hrt_absolute_time(); + + pthread_mutex_lock(&_mtx); + + while (true) { + + const hrt_abstime now = hrt_absolute_time(); + + /* call fsync periodically to minimize potential loss of data */ + const bool call_fsync = ++poll_count >= 100 || now - last_fsync > 1_s; + + if (call_fsync) { + last_fsync = now; + poll_count = 0; + } + + constexpr size_t min_available[(int)LogType::Count] = { + _min_write_chunk, + 1 // For the mission log, write as soon as there is data available + }; + + /* Check all buffers for available data. Mission log is first to avoid drops */ + int i = (int)LogType::Count - 1; + + while (i >= 0) { + void *read_ptr; + bool is_part; + LogFileBuffer &buffer = _buffers[i]; + size_t available = buffer.get_read_ptr(&read_ptr, &is_part); + + /* if sufficient data available or partial read or terminating, write data */ + if (available >= min_available[i] || is_part || (!buffer._should_run && available > 0)) { + pthread_mutex_unlock(&_mtx); + + written = buffer.write_to_file(read_ptr, available, call_fsync); + + /* buffer.mark_read() requires _mtx to be locked */ + pthread_mutex_lock(&_mtx); + + if (written >= 0) { + /* subtract bytes written from number in buffer (count -= written) */ + buffer.mark_read(written); + + if (!buffer._should_run && written == static_cast(available) && !is_part) { + /* Stop only when all data written */ + buffer.close_file(); + } + + } else { + PX4_ERR("write failed (%i)", errno); + buffer._should_run = false; + buffer.close_file(); + } + + } else if (call_fsync && buffer._should_run) { + pthread_mutex_unlock(&_mtx); + buffer.fsync(); + pthread_mutex_lock(&_mtx); + + } else if (available == 0 && !buffer._should_run) { + buffer.close_file(); + } + + /* if split into 2 parts, write the second part immediately as well */ + if (!is_part) { + --i; + } + } + + + if (_buffers[0].fd() < 0 && _buffers[1].fd() < 0) { + // stop when both files are closed + break; + } + + /* Wait for a call to notify(), which indicates new data is available. + * Note that at this point there could already be new data available (because of a longer write), + * and calling pthread_cond_wait() will still wait for the next notify(). But this is generally + * not an issue because notify() is called regularly. + * If the logger was switched off in the meantime, do not wait for data, instead run this loop + * once more to write remaining data and close the file. */ + if (_buffers[0]._should_run) { + pthread_cond_wait(&_cv, &_mtx); + } + } + + // go back to idle + pthread_mutex_unlock(&_mtx); + } +} + +int LogWriterFile::write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start) +{ + if (_need_reliable_transfer) { + int ret; + + // if there's a dropout, write it first (because we might split the message) + if (dropout_start) { + while ((ret = write(type, ptr, 0, dropout_start)) == -1) { + unlock(); + notify(); + px4_usleep(3000); + lock(); + } + } + + uint8_t *uptr = (uint8_t *)ptr; + + do { + // Split into several blocks if the data is longer than the write buffer + size_t write_size = math::min(size, _buffers[(int)type].buffer_size()); + + while ((ret = write(type, uptr, write_size, 0)) == -1) { + unlock(); + notify(); + px4_usleep(3000); + lock(); + } + + uptr += write_size; + size -= write_size; + } while (size > 0); + + return ret; + } + + return write(type, ptr, size, dropout_start); +} + +int LogWriterFile::write(LogType type, void *ptr, size_t size, uint64_t dropout_start) +{ + if (!is_started(type)) { + return 0; + } + + // Bytes available to write + size_t available = _buffers[(int)type].available(); + size_t dropout_size = 0; + + if (dropout_start) { + dropout_size = sizeof(ulog_message_dropout_s); + } + + if (size + dropout_size > available) { + // buffer overflow + return -1; + } + + if (dropout_start) { + //write dropout msg + ulog_message_dropout_s dropout_msg; + dropout_msg.duration = (uint16_t)(hrt_elapsed_time(&dropout_start) / 1000); + _buffers[(int)type].write_no_check(&dropout_msg, sizeof(dropout_msg)); + } + + _buffers[(int)type].write_no_check(ptr, size); + return 0; +} + +const char *log_type_str(LogType type) +{ + switch (type) { + case LogType::Full: return "full"; + + case LogType::Mission: return "mission"; + + case LogType::Count: break; + } + + return "unknown"; +} + +LogWriterFile::LogFileBuffer::LogFileBuffer(size_t log_buffer_size, perf_counter_t perf_write, + perf_counter_t perf_fsync) + : _buffer_size(log_buffer_size), _perf_write(perf_write), _perf_fsync(perf_fsync) +{ +} + +LogWriterFile::LogFileBuffer::~LogFileBuffer() +{ + if (_fd >= 0) { + close(_fd); + } + + free(_buffer); + + perf_free(_perf_write); + perf_free(_perf_fsync); +} + +void LogWriterFile::LogFileBuffer::write_no_check(void *ptr, size_t size) +{ + size_t n = _buffer_size - _head; // bytes to end of the buffer + + uint8_t *buffer_c = static_cast(ptr); + + if (size > n) { + // Message goes over the end of the buffer + memcpy(&(_buffer[_head]), buffer_c, n); + _head = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + size_t p = size - n; // number of bytes to write + memcpy(&(_buffer[_head]), &(buffer_c[n]), p); + _head = (_head + p) % _buffer_size; + _count += size; +} + +size_t LogWriterFile::LogFileBuffer::get_read_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int read_ptr = _head - _count; + + if (read_ptr < 0) { + read_ptr += _buffer_size; + *ptr = &_buffer[read_ptr]; + *is_part = true; + return _buffer_size - read_ptr; + + } else { + *ptr = &_buffer[read_ptr]; + *is_part = false; + return _count; + } +} + +bool LogWriterFile::LogFileBuffer::start_log(const char *filename) +{ + _fd = ::open(filename, O_CREAT | O_WRONLY, PX4_O_MODE_666); + + if (_fd < 0) { + PX4_ERR("Can't open log file %s, errno: %d", filename, errno); + return false; + } + + if (_buffer == nullptr) { + _buffer = (uint8_t *) px4_cache_aligned_alloc(_buffer_size); + + if (_buffer == nullptr) { + PX4_ERR("Can't create log buffer"); + ::close(_fd); + _fd = -1; + return false; + } + } + + // Clear buffer and counters + _head = 0; + _count = 0; + _total_written = 0; + + _should_run = true; + + return true; +} + +void LogWriterFile::LogFileBuffer::fsync() const +{ + perf_begin(_perf_fsync); + ::fsync(_fd); + perf_end(_perf_fsync); +} + +ssize_t LogWriterFile::LogFileBuffer::write_to_file(const void *buffer, size_t size, bool call_fsync) const +{ + perf_begin(_perf_write); + ssize_t ret = ::write(_fd, buffer, size); + perf_end(_perf_write); + + if (call_fsync) { + fsync(); + } + + return ret; +} + +void LogWriterFile::LogFileBuffer::close_file() +{ + _head = 0; + _count = 0; + + if (_fd >= 0) { + int res = close(_fd); + _fd = -1; + + if (res) { + PX4_WARN("closing log file failed (%i)", errno); + + } else { + PX4_INFO("closed logfile, bytes written: %zu", _total_written); + } + } +} + +} +} diff --git a/src/prometheus_px4/src/modules/logger/log_writer_file.h b/src/prometheus_px4/src/modules/logger/log_writer_file.h new file mode 100644 index 0000000..b3649b0 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/log_writer_file.h @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace px4 +{ +namespace logger +{ + +/** + * @enum LogType + * Defines different log (file) types + */ +enum class LogType { + Full = 0, //!< Normal, full size log + Mission, //!< reduced mission log (e.g. for geotagging) + + Count +}; + +const char *log_type_str(LogType type); + +/** + * @class LogWriterFile + * Writes logging data to a file + */ +class LogWriterFile +{ +public: + LogWriterFile(size_t buffer_size); + ~LogWriterFile(); + + bool init(); + + /** + * start the thread + * @return 0 on success, error number otherwise (@see pthread_create) + */ + int thread_start(); + + void thread_stop(); + + void start_log(LogType type, const char *filename); + + void stop_log(LogType type); + + bool is_started(LogType type) const { return _buffers[(int)type]._should_run; } + + /** @see LogWriter::write_message() */ + int write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start = 0); + + void lock() + { + pthread_mutex_lock(&_mtx); + } + + void unlock() + { + pthread_mutex_unlock(&_mtx); + } + + void notify() + { + pthread_cond_broadcast(&_cv); + } + + size_t get_total_written(LogType type) const + { + return _buffers[(int)type].total_written(); + } + + size_t get_buffer_size(LogType type) const + { + return _buffers[(int)type].buffer_size(); + } + + size_t get_buffer_fill_count(LogType type) const + { + return _buffers[(int)type].count(); + } + + void set_need_reliable_transfer(bool need_reliable) + { + _need_reliable_transfer = need_reliable; + } + + bool need_reliable_transfer() const + { + return _need_reliable_transfer; + } + + pthread_t thread_id() const { return _thread; } + +private: + static void *run_helper(void *); + + void run(); + + /** + * permanently store the ulog file name for the hardfault crash handler, so that it can + * append crash logs to the last ulog file. + * @param log_file path to the log file + * @return 0 on success, <0 errno otherwise + */ + int hardfault_store_filename(const char *log_file); + + /** + * write w/o waiting/blocking + */ + int write(LogType type, void *ptr, size_t size, uint64_t dropout_start); + + /* 512 didn't seem to work properly, 4096 should match the FAT cluster size */ + static constexpr size_t _min_write_chunk = 4096; + + class LogFileBuffer + { + public: + LogFileBuffer(size_t log_buffer_size, perf_counter_t perf_write, perf_counter_t perf_fsync); + + ~LogFileBuffer(); + + bool start_log(const char *filename); + + void close_file(); + + size_t get_read_ptr(void **ptr, bool *is_part); + + /** + * Write to the buffer but assuming there is enough space + */ + inline void write_no_check(void *ptr, size_t size); + + size_t available() const { return _buffer_size - _count; } + + int fd() const { return _fd; } + + inline ssize_t write_to_file(const void *buffer, size_t size, bool call_fsync) const; + + inline void fsync() const; + + void mark_read(size_t n) { _count -= n; _total_written += n; } + + size_t total_written() const { return _total_written; } + size_t buffer_size() const { return _buffer_size; } + size_t count() const { return _count; } + + bool _should_run = false; + + private: + const size_t _buffer_size; + int _fd = -1; + uint8_t *_buffer = nullptr; + size_t _head = 0; ///< next position to write to + size_t _count = 0; ///< number of bytes in _buffer to be written + size_t _total_written = 0; + perf_counter_t _perf_write; + perf_counter_t _perf_fsync; + }; + + LogFileBuffer _buffers[(int)LogType::Count]; + + bool _exit_thread = false; + bool _need_reliable_transfer = false; + pthread_mutex_t _mtx; + pthread_cond_t _cv; + pthread_t _thread = 0; +}; + +} +} diff --git a/src/prometheus_px4/src/modules/logger/log_writer_mavlink.cpp b/src/prometheus_px4/src/modules/logger/log_writer_mavlink.cpp new file mode 100644 index 0000000..1c2a7b4 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/log_writer_mavlink.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "log_writer_mavlink.h" +#include "messages.h" + +#include +#include +#include +#include +#include + +namespace px4 +{ +namespace logger +{ + +LogWriterMavlink::LogWriterMavlink() +{ + _ulog_stream_data.length = 0; +} + +bool LogWriterMavlink::init() +{ + return true; +} + +LogWriterMavlink::~LogWriterMavlink() +{ + if (_ulog_stream_ack_sub >= 0) { + orb_unsubscribe(_ulog_stream_ack_sub); + } +} + +void LogWriterMavlink::start_log() +{ + if (_ulog_stream_ack_sub == -1) { + _ulog_stream_ack_sub = orb_subscribe(ORB_ID(ulog_stream_ack)); + } + + // make sure we don't get any stale ack's by doing an orb_copy + ulog_stream_ack_s ack; + orb_copy(ORB_ID(ulog_stream_ack), _ulog_stream_ack_sub, &ack); + + _ulog_stream_data.msg_sequence = 0; + _ulog_stream_data.length = 0; + _ulog_stream_data.first_message_offset = 0; + + _is_started = true; +} + +void LogWriterMavlink::stop_log() +{ + _ulog_stream_data.length = 0; + _is_started = false; +} + +int LogWriterMavlink::write_message(void *ptr, size_t size) +{ + if (!is_started()) { + return 0; + } + + const uint8_t data_len = (uint8_t)sizeof(_ulog_stream_data.data); + uint8_t *ptr_data = (uint8_t *)ptr; + + if (_ulog_stream_data.first_message_offset == 255) { + _ulog_stream_data.first_message_offset = _ulog_stream_data.length; + } + + while (size > 0) { + size_t send_len = math::min((size_t)data_len - _ulog_stream_data.length, size); + memcpy(_ulog_stream_data.data + _ulog_stream_data.length, ptr_data, send_len); + _ulog_stream_data.length += send_len; + ptr_data += send_len; + size -= send_len; + + if (_ulog_stream_data.length >= data_len) { + if (publish_message()) { + return -2; + } + } + } + + return 0; +} + +void LogWriterMavlink::set_need_reliable_transfer(bool need_reliable) +{ + if (!need_reliable && _need_reliable_transfer) { + if (_ulog_stream_data.length > 0) { + // make sure to send previous data using reliable transfer + publish_message(); + } + } + + _need_reliable_transfer = need_reliable; +} + +int LogWriterMavlink::publish_message() +{ + _ulog_stream_data.timestamp = hrt_absolute_time(); + _ulog_stream_data.flags = 0; + + if (_need_reliable_transfer) { + _ulog_stream_data.flags = _ulog_stream_data.FLAGS_NEED_ACK; + } + + _ulog_stream_pub.publish(_ulog_stream_data); + + if (_need_reliable_transfer) { + // we need to wait for an ack. Note that this blocks the main logger thread, so if a file logging + // is already running, it will miss samples. + px4_pollfd_struct_t fds[1]; + fds[0].fd = _ulog_stream_ack_sub; + fds[0].events = POLLIN; + bool got_ack = false; + const int timeout_ms = ulog_stream_ack_s::ACK_TIMEOUT * ulog_stream_ack_s::ACK_MAX_TRIES; + + hrt_abstime started = hrt_absolute_time(); + + do { + int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_ms); + + if (ret <= 0) { + break; + } + + if (fds[0].revents & POLLIN) { + ulog_stream_ack_s ack; + orb_copy(ORB_ID(ulog_stream_ack), _ulog_stream_ack_sub, &ack); + + if (ack.msg_sequence == _ulog_stream_data.msg_sequence) { + got_ack = true; + } + + } else { + break; + } + } while (!got_ack && hrt_elapsed_time(&started) / 1000 < timeout_ms); + + if (!got_ack) { + PX4_ERR("Ack timeout. Stopping mavlink log"); + stop_log(); + return -2; + } + + PX4_DEBUG("got ack in %i ms", (int)(hrt_elapsed_time(&started) / 1000)); + } + + _ulog_stream_data.msg_sequence++; + _ulog_stream_data.length = 0; + _ulog_stream_data.first_message_offset = 255; + return 0; +} + +} +} diff --git a/src/prometheus_px4/src/modules/logger/log_writer_mavlink.h b/src/prometheus_px4/src/modules/logger/log_writer_mavlink.h new file mode 100644 index 0000000..ad6d80f --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/log_writer_mavlink.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +namespace px4 +{ +namespace logger +{ + +/** + * @class LogWriterMavlink + * Writes logging data to uORB, and then sent via mavlink + */ +class LogWriterMavlink +{ +public: + LogWriterMavlink(); + ~LogWriterMavlink(); + + bool init(); + + void start_log(); + + void stop_log(); + + bool is_started() const { return _is_started; } + + /** @see LogWriter::write_message() */ + int write_message(void *ptr, size_t size); + + void set_need_reliable_transfer(bool need_reliable); + + bool need_reliable_transfer() const + { + return _need_reliable_transfer; + } + +private: + + /** publish message, wait for ack if needed & reset message */ + int publish_message(); + + ulog_stream_s _ulog_stream_data{}; + uORB::Publication _ulog_stream_pub{ORB_ID(ulog_stream)}; + int _ulog_stream_ack_sub{-1}; + bool _need_reliable_transfer{false}; + bool _is_started{false}; +}; + +} +} diff --git a/src/prometheus_px4/src/modules/logger/logged_topics.cpp b/src/prometheus_px4/src/modules/logger/logged_topics.cpp new file mode 100644 index 0000000..b668559 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/logged_topics.cpp @@ -0,0 +1,482 @@ +/**************************************************************************** + * + * 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 "logged_topics.h" +#include "messages.h" + +#include +#include +#include +#include + +#include + +using namespace px4::logger; + +void LoggedTopics::add_default_topics() +{ + add_topic("actuator_armed"); + add_topic("actuator_controls_0", 50); + add_topic("actuator_controls_1", 100); + add_topic("actuator_controls_2", 100); + add_topic("actuator_controls_3", 100); + add_topic("actuator_controls_4", 100); + add_topic("actuator_controls_5", 100); + add_topic("airspeed", 1000); + add_topic("airspeed_validated", 200); + add_topic("camera_capture"); + add_topic("camera_trigger"); + add_topic("camera_trigger_secondary"); + add_topic("cellular_status", 200); + add_topic("commander_state"); + add_topic("cpuload"); + add_topic("esc_status", 250); + add_topic("follow_target", 500); + add_topic("generator_status"); + add_topic("heater_status"); + add_topic("home_position"); + add_topic("hover_thrust_estimate", 100); + add_topic("input_rc", 500); + add_topic("mag_worker_data"); + add_topic("manual_control_setpoint", 200); + add_topic("manual_control_switches"); + add_topic("mission"); + add_topic("mission_result"); + add_topic("navigator_mission_item"); + add_topic("offboard_control_mode", 100); + add_topic("onboard_computer_status", 10); + add_topic("parameter_update"); + add_topic("position_controller_status", 500); + add_topic("position_setpoint_triplet", 200); + add_topic("px4io_status"); + add_topic("radio_status"); + add_topic("rpm", 500); + add_topic("rtl_flight_time", 1000); + add_topic("safety"); + add_topic("sensor_combined"); + add_topic("sensor_correction"); + add_topic("sensor_gyro_fft", 50); + add_topic("sensor_preflight_mag", 500); + add_topic("sensor_selection"); + add_topic("sensors_status_imu", 200); + add_topic("system_power", 500); + add_topic("takeoff_status", 1000); + add_topic("tecs_status", 200); + add_topic("test_motor", 500); + add_topic("trajectory_setpoint", 200); + add_topic("transponder_report"); + add_topic("vehicle_acceleration", 50); + add_topic("vehicle_air_data", 200); + add_topic("vehicle_angular_velocity", 20); + add_topic("vehicle_attitude", 50); + add_topic("vehicle_attitude_setpoint", 50); + add_topic("vehicle_command"); + add_topic("vehicle_constraints", 1000); + add_topic("vehicle_control_mode"); + add_topic("vehicle_global_position", 200); + add_topic("vehicle_gps_position", 500); + add_topic("vehicle_land_detected"); + add_topic("vehicle_local_position", 100); + add_topic("vehicle_local_position_setpoint", 100); + add_topic("vehicle_magnetometer", 200); + add_topic("vehicle_rates_setpoint", 20); + add_topic("vehicle_roi", 1000); + add_topic("vehicle_status"); + add_topic("vehicle_status_flags"); + add_topic("vtol_vehicle_status", 200); + add_topic("wind", 1000); + + // Control allocation topics + add_topic("vehicle_actuator_setpoint", 20); + add_topic("vehicle_angular_acceleration", 20); + add_topic("vehicle_angular_acceleration_setpoint", 20); + add_topic("vehicle_thrust_setpoint", 20); + add_topic("vehicle_torque_setpoint", 20); + + // multi topics + add_topic_multi("actuator_outputs", 100, 3); + add_topic_multi("airspeed_wind", 1000); + add_topic_multi("logger_status", 0, 2); + add_topic_multi("multirotor_motor_limits", 1000, 2); + add_topic_multi("rate_ctrl_status", 200, 2); + add_topic_multi("telemetry_status", 1000, 4); + + // EKF multi topics (currently max 9 estimators) +#if CONSTRAINED_MEMORY + static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1; +#else + static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificailly limited until PlotJuggler fixed + add_topic("estimator_selector_status"); + add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); +#endif + + add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); + + // log all raw sensors at minimal rate (at least 1 Hz) + add_topic_multi("battery_status", 200, 2); + add_topic_multi("differential_pressure", 1000, 2); + add_topic_multi("distance_sensor", 1000); + add_topic_multi("optical_flow", 1000, 1); + add_topic_multi("sensor_accel", 1000, 4); + add_topic_multi("sensor_baro", 1000, 4); + add_topic_multi("sensor_gps", 1000, 2); + add_topic_multi("sensor_gyro", 1000, 4); + add_topic_multi("sensor_mag", 1000, 4); + add_topic_multi("vehicle_imu", 500, 4); + add_topic_multi("vehicle_imu_status", 1000, 4); + add_topic_multi("vehicle_magnetometer", 500, 4); + +#ifdef CONFIG_ARCH_BOARD_PX4_SITL + add_topic("actuator_controls_virtual_fw"); + add_topic("actuator_controls_virtual_mc"); + add_topic("fw_virtual_attitude_setpoint"); + add_topic("mc_virtual_attitude_setpoint"); + add_topic("time_offset"); + add_topic("vehicle_angular_acceleration", 10); + add_topic("vehicle_angular_velocity", 10); + add_topic("vehicle_attitude_groundtruth", 10); + add_topic("vehicle_global_position_groundtruth", 100); + add_topic("vehicle_local_position_groundtruth", 100); +#endif /* CONFIG_ARCH_BOARD_PX4_SITL */ + + int32_t gps_dump_comm = 0; + param_get(param_find("GPS_DUMP_COMM"), &gps_dump_comm); + + if (gps_dump_comm == 1) { + add_topic("gps_dump"); + } +} + +void LoggedTopics::add_high_rate_topics() +{ + // maximum rate to analyze fast maneuvers (e.g. for racing) + add_topic("actuator_controls_0"); + add_topic("actuator_outputs"); + add_topic("manual_control_setpoint"); + add_topic("rate_ctrl_status", 20); + add_topic("sensor_combined"); + add_topic("vehicle_angular_acceleration"); + add_topic("vehicle_angular_velocity"); + add_topic("vehicle_attitude"); + add_topic("vehicle_attitude_setpoint"); + add_topic("vehicle_rates_setpoint"); +} + +void LoggedTopics::add_debug_topics() +{ + add_topic("debug_array"); + add_topic("debug_key_value"); + add_topic("debug_value"); + add_topic("debug_vect"); + add_topic_multi("satellite_info", 1000, 2); +} + +void LoggedTopics::add_estimator_replay_topics() +{ + // for estimator replay (need to be at full rate) + add_topic("ekf2_timestamps"); + + // current EKF2 subscriptions + add_topic("airspeed"); + add_topic("optical_flow"); + add_topic("sensor_combined"); + add_topic("sensor_selection"); + add_topic("vehicle_air_data"); + add_topic("vehicle_gps_position"); + add_topic("vehicle_land_detected"); + add_topic("vehicle_magnetometer"); + add_topic("vehicle_status"); + add_topic("vehicle_visual_odometry"); + add_topic_multi("distance_sensor"); +} + +void LoggedTopics::add_thermal_calibration_topics() +{ + add_topic_multi("sensor_accel", 100, 3); + add_topic_multi("sensor_baro", 100, 3); + add_topic_multi("sensor_gyro", 100, 3); +} + +void LoggedTopics::add_sensor_comparison_topics() +{ + add_topic_multi("sensor_accel", 100, 3); + add_topic_multi("sensor_baro", 100, 3); + add_topic_multi("sensor_gyro", 100, 3); + add_topic_multi("sensor_mag", 100, 4); +} + +void LoggedTopics::add_vision_and_avoidance_topics() +{ + add_topic("collision_constraints"); + add_topic("obstacle_distance_fused"); + add_topic("vehicle_mocap_odometry", 30); + add_topic("vehicle_trajectory_waypoint", 200); + add_topic("vehicle_trajectory_waypoint_desired", 200); + add_topic("vehicle_visual_odometry", 30); +} + +void LoggedTopics::add_raw_imu_gyro_fifo() +{ + add_topic("sensor_gyro_fifo"); +} + +void LoggedTopics::add_raw_imu_accel_fifo() +{ + add_topic("sensor_accel_fifo"); +} + +void LoggedTopics::add_system_identification_topics() +{ + // for system id need to log imu and controls at full rate + add_topic("actuator_controls_0"); + add_topic("actuator_controls_1"); + add_topic("sensor_combined"); + add_topic("vehicle_angular_acceleration"); + add_topic("vehicle_angular_acceleration_setpoint"); + add_topic("vehicle_torque_setpoint"); +} + +int LoggedTopics::add_topics_from_file(const char *fname) +{ + int ntopics = 0; + + /* open the topic list file */ + FILE *fp = fopen(fname, "r"); + + if (fp == nullptr) { + return -1; + } + + /* call add_topic for each topic line in the file */ + for (;;) { + /* get a line, bail on error/EOF */ + char line[80]; + line[0] = '\0'; + + if (fgets(line, sizeof(line), fp) == nullptr) { + break; + } + + /* skip comment lines */ + if ((strlen(line) < 2) || (line[0] == '#')) { + continue; + } + + // read line with format: [ [ ]] + char topic_name[80]; + uint32_t interval_ms = 0; + uint32_t instance = 0; + int nfields = sscanf(line, "%s %" PRIu32 " %" PRIu32, topic_name, &interval_ms, &instance); + + if (nfields > 0) { + int name_len = strlen(topic_name); + + if (name_len > 0 && topic_name[name_len - 1] == ',') { + topic_name[name_len - 1] = '\0'; + } + + /* add topic with specified interval_ms */ + if ((nfields > 2 && add_topic(topic_name, interval_ms, instance)) + || add_topic_multi(topic_name, interval_ms)) { + ntopics++; + + } else { + PX4_ERR("Failed to add topic %s", topic_name); + } + } + } + + fclose(fp); + return ntopics; +} + +void LoggedTopics::initialize_mission_topics(MissionLogType mission_log_type) +{ + if (mission_log_type == MissionLogType::Complete) { + add_mission_topic("camera_capture"); + add_mission_topic("mission_result"); + add_mission_topic("vehicle_global_position", 1000); + add_mission_topic("vehicle_status", 1000); + + } else if (mission_log_type == MissionLogType::Geotagging) { + add_mission_topic("camera_capture"); + } +} + +void LoggedTopics::add_mission_topic(const char *name, uint16_t interval_ms) +{ + if (add_topic(name, interval_ms)) { + ++_num_mission_subs; + } +} + +bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, uint8_t instance) +{ + size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':' + + if (fields_len > sizeof(ulog_message_format_s::format)) { + PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len, + sizeof(ulog_message_format_s::format)); + + return false; + } + + if (_subscriptions.count >= MAX_TOPICS_NUM) { + PX4_WARN("Too many subscriptions, failed to add: %s %" PRIu8, topic->o_name, instance); + return false; + } + + RequestedSubscription &sub = _subscriptions.sub[_subscriptions.count++]; + sub.interval_ms = interval_ms; + sub.instance = instance; + sub.id = static_cast(topic->o_id); + return true; +} + +bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t instance) +{ + const orb_metadata *const *topics = orb_get_topics(); + bool success = false; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(name, topics[i]->o_name) == 0) { + bool already_added = false; + + // check if already added: if so, only update the interval + for (int j = 0; j < _subscriptions.count; ++j) { + if (_subscriptions.sub[j].id == static_cast(topics[i]->o_id) && + _subscriptions.sub[j].instance == instance) { + + PX4_DEBUG("logging topic %s(%" PRUu8 "), interval: %" PRUu16 ", already added, only setting interval", + topics[i]->o_name, instance, interval_ms); + + _subscriptions.sub[j].interval_ms = interval_ms; + success = true; + already_added = true; + break; + } + } + + if (!already_added) { + success = add_topic(topics[i], interval_ms, instance); + PX4_DEBUG("logging topic: %s(%" PRUu8 "), interval: %" PRUu16, topics[i]->o_name, instance, interval_ms); + break; + } + } + } + + return success; +} + +bool LoggedTopics::add_topic_multi(const char *name, uint16_t interval_ms, uint8_t max_num_instances) +{ + // add all possible instances + for (uint8_t instance = 0; instance < max_num_instances; instance++) { + add_topic(name, interval_ms, instance); + } + + return true; +} + +bool LoggedTopics::initialize_logged_topics(SDLogProfileMask profile) +{ + int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt"); + + if (ntopics > 0) { + PX4_INFO("logging %d topics from logger_topics.txt", ntopics); + + } else { + initialize_configured_topics(profile); + } + + return _subscriptions.count > 0; +} + +void LoggedTopics::initialize_configured_topics(SDLogProfileMask profile) +{ + // load appropriate topics for profile + // the order matters: if several profiles add the same topic, the logging rate of the last one will be used + if (profile & SDLogProfileMask::DEFAULT) { + add_default_topics(); + } + + if (profile & SDLogProfileMask::ESTIMATOR_REPLAY) { + add_estimator_replay_topics(); + } + + if (profile & SDLogProfileMask::THERMAL_CALIBRATION) { + add_thermal_calibration_topics(); + } + + if (profile & SDLogProfileMask::SYSTEM_IDENTIFICATION) { + add_system_identification_topics(); + } + + if (profile & SDLogProfileMask::HIGH_RATE) { + add_high_rate_topics(); + } + + if (profile & SDLogProfileMask::DEBUG_TOPICS) { + add_debug_topics(); + } + + if (profile & SDLogProfileMask::SENSOR_COMPARISON) { + add_sensor_comparison_topics(); + } + + if (profile & SDLogProfileMask::VISION_AND_AVOIDANCE) { + add_vision_and_avoidance_topics(); + } + + if (profile & SDLogProfileMask::RAW_IMU_GYRO_FIFO) { + add_raw_imu_gyro_fifo(); + } + + if (profile & SDLogProfileMask::RAW_IMU_ACCEL_FIFO) { + add_raw_imu_accel_fifo(); + } +} diff --git a/src/prometheus_px4/src/modules/logger/logged_topics.h b/src/prometheus_px4/src/modules/logger/logged_topics.h new file mode 100644 index 0000000..6cf4cfc --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/logged_topics.h @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include + +namespace px4 +{ +namespace logger +{ + +enum class SDLogProfileMask : int32_t { + DEFAULT = 1 << 0, + ESTIMATOR_REPLAY = 1 << 1, + THERMAL_CALIBRATION = 1 << 2, + SYSTEM_IDENTIFICATION = 1 << 3, + HIGH_RATE = 1 << 4, + DEBUG_TOPICS = 1 << 5, + SENSOR_COMPARISON = 1 << 6, + VISION_AND_AVOIDANCE = 1 << 7, + RAW_IMU_GYRO_FIFO = 1 << 8, + RAW_IMU_ACCEL_FIFO = 1 << 9 +}; + +enum class MissionLogType : int32_t { + Disabled = 0, + Complete = 1, + Geotagging = 2 +}; + +inline bool operator&(SDLogProfileMask a, SDLogProfileMask b) +{ + return static_cast(a) & static_cast(b); +} + +/** + * @class LoggedTopics + * Contains the list of configured topics + */ +class LoggedTopics +{ +public: + static constexpr int MAX_TOPICS_NUM = 255; /**< Maximum number of logged topics */ + + struct RequestedSubscription { + uint16_t interval_ms; + uint8_t instance; + ORB_ID id{ORB_ID::INVALID}; + }; + struct RequestedSubscriptionArray { + RequestedSubscription sub[MAX_TOPICS_NUM]; + int count{0}; + }; + + LoggedTopics() = default; + ~LoggedTopics() = default; + + /** + * Add topic subscriptions based on the configured mission log type. + * Must be initialized first + */ + void initialize_mission_topics(MissionLogType mission_log_type); + + bool initialize_logged_topics(SDLogProfileMask profile); + + const RequestedSubscriptionArray &subscriptions() const { return _subscriptions; } + int numMissionSubscriptions() const { return _num_mission_subs; } + +private: + + /** + * Add a topic to be logged. + * @param name topic name + * @param interval limit in milliseconds if >0, otherwise log as fast as the topic is updated. + * @param instance orb topic instance + * @return true on success + */ + bool add_topic(const char *name, uint16_t interval_ms = 0, uint8_t instance = 0); + + /** + * Add a topic to be logged. + * @param name topic name + * @param interval limit in milliseconds if >0, otherwise log as fast as the topic is updated. + * @param instance orb topic instance + * @param max_num_instances the max multi-instance to add. + * @return true on success + */ + bool add_topic_multi(const char *name, uint16_t interval_ms = 0, uint8_t max_num_instances = ORB_MULTI_MAX_INSTANCES); + + /** + * Parse a file containing a list of uORB topics to log, calling add_topic for each + * @param fname name of file + * @return number of topics added + */ + int add_topics_from_file(const char *fname); + + /** + * Add a topic to be logged for the mission log (it's also added to the full log). + * The interval is expected to be 0 or large (in the order of 0.1 seconds or higher). + * Must be called before all other topics are added. + * @param name topic name + * @param interval limit rate if >0 [ms], otherwise log as fast as the topic is updated. + */ + void add_mission_topic(const char *name, uint16_t interval_ms = 0); + + /** + * Add topic subscriptions based on the profile configuration + */ + void initialize_configured_topics(SDLogProfileMask profile); + + void add_default_topics(); + void add_estimator_replay_topics(); + void add_thermal_calibration_topics(); + void add_system_identification_topics(); + void add_high_rate_topics(); + void add_debug_topics(); + void add_sensor_comparison_topics(); + void add_vision_and_avoidance_topics(); + void add_raw_imu_gyro_fifo(); + void add_raw_imu_accel_fifo(); + + /** + * add a logged topic (called by add_topic() above). + * @return true on success + */ + bool add_topic(const orb_metadata *topic, uint16_t interval_ms = 0, uint8_t instance = 0); + + RequestedSubscriptionArray _subscriptions; + int _num_mission_subs{0}; +}; + +} //namespace logger +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/logger/logger.cpp b/src/prometheus_px4/src/modules/logger/logger.cpp new file mode 100644 index 0000000..e748dd5 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/logger.cpp @@ -0,0 +1,2143 @@ +/**************************************************************************** + * + * Copyright (c) 2016, 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 +#include +#include "logged_topics.h" +#include "logger.h" +#include "messages.h" +#include "watchdog.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define DBGPRINT //write status output every few seconds + +#if defined(DBGPRINT) +// needed for mallinfo +#if defined(__PX4_POSIX) && !defined(__PX4_DARWIN) +#include +#endif /* __PX4_POSIX */ + +// struct mallinfo not available on OSX? +#if defined(__PX4_DARWIN) +#undef DBGPRINT +#endif /* defined(__PX4_DARWIN) */ +#endif /* defined(DBGPRINT) */ + +using namespace px4::logger; +using namespace time_literals; + + +struct timer_callback_data_s { + px4_sem_t semaphore; + + watchdog_data_t watchdog_data; + volatile bool watchdog_triggered = false; +}; + +/* This is used to schedule work for the logger (periodic scan for updated topics) */ +static void timer_callback(void *arg) +{ + /* Note: we are in IRQ context here (on NuttX) */ + + timer_callback_data_s *data = (timer_callback_data_s *)arg; + + if (watchdog_update(data->watchdog_data)) { + data->watchdog_triggered = true; + } + + /* check the value of the semaphore: if the logger cannot keep up with running it's main loop as fast + * as the timer_callback here increases the semaphore count, the counter would increase unbounded, + * leading to an overflow at some point. This case we want to avoid here, so we check the current + * value against a (somewhat arbitrary) threshold, and avoid calling sem_post() if it's exceeded. + * (it's not a problem if the threshold is a bit too large, it just means the logger will do + * multiple iterations at once, the next time it's scheduled). */ + int semaphore_value; + + if (px4_sem_getvalue(&data->semaphore, &semaphore_value) == 0 && semaphore_value > 1) { + return; + } + + px4_sem_post(&data->semaphore); + +} + + +int logger_main(int argc, char *argv[]) +{ + // logger currently assumes little endian + int num = 1; + + if (*(char *)&num != 1) { + PX4_ERR("Logger only works on little endian!\n"); + return 1; + } + + return Logger::main(argc, argv); +} + +namespace px4 +{ +namespace logger +{ + +constexpr const char *Logger::LOG_ROOT[(int)LogType::Count]; + +int Logger::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + print_usage("logger not running"); + return 1; + } + + if (!strcmp(argv[0], "on")) { + get_instance()->set_arm_override(true); + return 0; + } + + if (!strcmp(argv[0], "off")) { + get_instance()->set_arm_override(false); + return 0; + } + + return print_usage("unknown command"); +} + +int Logger::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("logger", + SCHED_DEFAULT, + SCHED_PRIORITY_LOG_CAPTURE, + 3700, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +int Logger::print_status() +{ + PX4_INFO("Running in mode: %s", configured_backend_mode()); + PX4_INFO("Number of subscriptions: %i (%i bytes)", _num_subscriptions, + (int)(_num_subscriptions * sizeof(LoggerSubscription))); + + bool is_logging = false; + + if (_writer.is_started(LogType::Full, LogWriter::BackendFile)) { + PX4_INFO("Full File Logging Running:"); + print_statistics(LogType::Full); + is_logging = true; + } + + if (_writer.is_started(LogType::Mission, LogWriter::BackendFile)) { + PX4_INFO("Mission File Logging Running:"); + print_statistics(LogType::Mission); + is_logging = true; + } + + if (_writer.is_started(LogType::Full, LogWriter::BackendMavlink)) { + PX4_INFO("Mavlink Logging Running (Full log)"); + is_logging = true; + } + + if (!is_logging) { + PX4_INFO("Not logging"); + } + + return 0; +} + +void Logger::print_statistics(LogType type) +{ + if (!_writer.is_started(type, LogWriter::BackendFile)) { //currently only statistics for file logging + return; + } + + Statistics &stats = _statistics[(int)type]; + + /* this is only for the file backend */ + float kibibytes = _writer.get_total_written_file(type) / 1024.0f; + float mebibytes = kibibytes / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - stats.start_time_file)) / 1000000.0f; + + PX4_INFO("Log file: %s/%s/%s", LOG_ROOT[(int)type], _file_name[(int)type].log_dir, _file_name[(int)type].log_file_name); + + if (mebibytes < 0.1f) { + PX4_INFO("Wrote %4.2f KiB (avg %5.2f KiB/s)", (double)kibibytes, (double)(kibibytes / seconds)); + + } else { + PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds)); + } + + PX4_INFO("Since last status: dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B", + stats.write_dropouts, (double)stats.max_dropout_duration, stats.high_water, _writer.get_buffer_size_file(type)); + stats.high_water = 0; + stats.write_dropouts = 0; + stats.max_dropout_duration = 0.f; +} + +Logger *Logger::instantiate(int argc, char *argv[]) +{ + uint32_t log_interval = 3500; + int log_buffer_size = 12 * 1024; + Logger::LogMode log_mode = Logger::LogMode::while_armed; + bool error_flag = false; + bool log_name_timestamp = false; + LogWriter::Backend backend = LogWriter::BackendAll; + const char *poll_topic = nullptr; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "r:b:etfm:p:x", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'r': { + unsigned long r = strtoul(myoptarg, nullptr, 10); + + if (r <= 0) { + r = 1e6; + } + + log_interval = 1e6 / r; + } + break; + + case 'x': + log_mode = Logger::LogMode::rc_aux1; + break; + + case 'e': + if (log_mode != Logger::LogMode::boot_until_shutdown) { + //setting boot_until_shutdown can't lower mode to boot_until_disarm + log_mode = Logger::LogMode::boot_until_disarm; + + } + + break; + + case 'f': + log_mode = Logger::LogMode::boot_until_shutdown; + break; + + case 'b': { + unsigned long s = strtoul(myoptarg, nullptr, 10); + + if (s < 1) { + s = 1; + } + + log_buffer_size = 1024 * s; + } + break; + + case 't': + log_name_timestamp = true; + break; + + + case 'm': + if (!strcmp(myoptarg, "file")) { + backend = LogWriter::BackendFile; + + } else if (!strcmp(myoptarg, "mavlink")) { + backend = LogWriter::BackendMavlink; + + } else if (!strcmp(myoptarg, "all")) { + backend = LogWriter::BackendAll; + + } else { + PX4_ERR("unknown mode: %s", myoptarg); + error_flag = true; + } + + break; + + case 'p': + poll_topic = myoptarg; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return nullptr; + } + + Logger *logger = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_mode, log_name_timestamp); + +#if defined(DBGPRINT) && defined(__PX4_NUTTX) + struct mallinfo alloc_info = mallinfo(); + PX4_INFO("largest free chunk: %d bytes", alloc_info.mxordblk); + PX4_INFO("remaining free heap: %d bytes", alloc_info.fordblks); +#endif /* DBGPRINT */ + + if (logger == nullptr) { + PX4_ERR("alloc failed"); + + } else { +#ifndef __PX4_NUTTX + //check for replay mode + const char *logfile = getenv(px4::replay::ENV_FILENAME); + + if (logfile) { + logger->setReplayFile(logfile); + } + +#endif /* __PX4_NUTTX */ + + } + + return logger; +} + +Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, + LogMode log_mode, bool log_name_timestamp) : + ModuleParams(nullptr), + _log_mode(log_mode), + _log_name_timestamp(log_name_timestamp), + _writer(backend, buffer_size), + _log_interval(log_interval) +{ + if (poll_topic_name) { + const orb_metadata *const *topics = orb_get_topics(); + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(poll_topic_name, topics[i]->o_name) == 0) { + _polling_topic_meta = topics[i]; + break; + } + } + + if (!_polling_topic_meta) { + PX4_ERR("Failed to find topic %s", poll_topic_name); + } + } +} + +Logger::~Logger() +{ + if (_replay_file_name) { + free(_replay_file_name); + } + + delete[](_msg_buffer); + delete[](_subscriptions); +} + +void Logger::update_params() +{ + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + } +} + +bool Logger::request_stop_static() +{ + if (is_running()) { + get_instance()->request_stop(); + return false; + } + + return true; +} + +bool Logger::copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe) +{ + LoggerSubscription &sub = _subscriptions[sub_idx]; + + bool updated = false; + + if (sub.valid()) { + if (sub.get_interval_us() == 0) { + // record gaps in full rate (no interval) messages + const unsigned last_generation = sub.get_last_generation(); + updated = sub.update(buffer); + + if (updated && (sub.get_last_generation() != last_generation + 1)) { + // error, missed a message + _message_gaps++; + } + + } else { + updated = sub.update(buffer); + } + + } else if (try_to_subscribe) { + if (sub.subscribe()) { + write_add_logged_msg(LogType::Full, sub); + + if (sub_idx < _num_mission_subs) { + write_add_logged_msg(LogType::Mission, sub); + } + + // copy first data + updated = sub.copy(buffer); + } + } + + return updated; +} + +const char *Logger::configured_backend_mode() const +{ + switch (_writer.backend()) { + case LogWriter::BackendFile: return "file"; + + case LogWriter::BackendMavlink: return "mavlink"; + + case LogWriter::BackendAll: return "all"; + + default: return "several"; + } +} + +bool Logger::initialize_topics() +{ + // get the logging profile + SDLogProfileMask sdlog_profile = (SDLogProfileMask)_param_sdlog_profile.get(); + + if ((int32_t)sdlog_profile == 0) { + PX4_WARN("No logging profile selected. Using default set"); + sdlog_profile = SDLogProfileMask::DEFAULT; + } + + LoggedTopics logged_topics; + + // initialize mission topics + logged_topics.initialize_mission_topics((MissionLogType)_param_sdlog_mission.get()); + _num_mission_subs = logged_topics.numMissionSubscriptions(); + + if (_num_mission_subs > 0) { + if (_num_mission_subs >= MAX_MISSION_TOPICS_NUM) { + PX4_ERR("Max num mission topics exceeded (%i)", _num_mission_subs); + _num_mission_subs = MAX_MISSION_TOPICS_NUM; + } + + for (int i = 0; i < _num_mission_subs; ++i) { + _mission_subscriptions[i].min_delta_ms = logged_topics.subscriptions().sub[i].interval_ms; + _mission_subscriptions[i].next_write_time = 0; + } + + int mkdir_ret = mkdir(LOG_ROOT[(int)LogType::Mission], S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret != 0 && errno != EEXIST) { + PX4_ERR("failed creating log root dir: %s (%i)", LOG_ROOT[(int)LogType::Mission], errno); + } + } + + if (!logged_topics.initialize_logged_topics(sdlog_profile)) { + return false; + } + + if ((sdlog_profile & SDLogProfileMask::RAW_IMU_ACCEL_FIFO) || (sdlog_profile & SDLogProfileMask::RAW_IMU_GYRO_FIFO)) { + // if we are logging high-rate FIFO, reduce the logging interval & increase process priority to avoid missing samples + PX4_INFO("Logging FIFO data: increasing task prio and logging rate"); + _log_interval = 800; + sched_param param{}; + param.sched_priority = SCHED_PRIORITY_ATTITUDE_CONTROL; + int ret = pthread_setschedparam(pthread_self(), SCHED_DEFAULT, ¶m); + + if (ret != 0) { + PX4_ERR("pthread_setschedparam failed (%i)", ret); + } + } + + delete[](_subscriptions); + _subscriptions = nullptr; + + if (logged_topics.subscriptions().count > 0) { + _subscriptions = new LoggerSubscription[logged_topics.subscriptions().count]; + + if (!_subscriptions) { + PX4_ERR("alloc failed"); + return false; + } + + for (int i = 0; i < logged_topics.subscriptions().count; ++i) { + const LoggedTopics::RequestedSubscription &sub = logged_topics.subscriptions().sub[i]; + _subscriptions[i] = LoggerSubscription(sub.id, sub.interval_ms, sub.instance); + _subscriptions[i].subscribe(); + } + } + + _num_subscriptions = logged_topics.subscriptions().count; + return true; +} + +void Logger::run() +{ + PX4_INFO("logger started (mode=%s)", configured_backend_mode()); + + if (_writer.backend() & LogWriter::BackendFile) { + int mkdir_ret = mkdir(LOG_ROOT[(int)LogType::Full], S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + PX4_INFO("log root dir created: %s", LOG_ROOT[(int)LogType::Full]); + + } else if (errno != EEXIST) { + PX4_ERR("failed creating log root dir: %s (%i)", LOG_ROOT[(int)LogType::Full], errno); + + if ((_writer.backend() & ~LogWriter::BackendFile) == 0) { + return; + } + } + + if (util::check_free_space(LOG_ROOT[(int)LogType::Full], _param_sdlog_dirs_max.get(), _mavlink_log_pub, + _file_name[(int)LogType::Full].sess_dir_index) == 1) { + return; + } + } + + uORB::Subscription parameter_update_sub(ORB_ID(parameter_update)); + + if (!initialize_topics()) { + return; + } + + //all topics added. Get required message buffer size + int max_msg_size = 0; + + for (int sub = 0; sub < _num_subscriptions; ++sub) { + //use o_size, because that's what orb_copy will use + if (_subscriptions[sub].get_topic()->o_size > max_msg_size) { + max_msg_size = _subscriptions[sub].get_topic()->o_size; + } + } + + max_msg_size += sizeof(ulog_message_data_header_s); + + if (sizeof(ulog_message_logging_s) > (size_t)max_msg_size) { + max_msg_size = sizeof(ulog_message_logging_s); + } + + if (_polling_topic_meta && _polling_topic_meta->o_size > max_msg_size) { + max_msg_size = _polling_topic_meta->o_size; + } + + if (max_msg_size > _msg_buffer_len) { + if (_msg_buffer) { + delete[](_msg_buffer); + } + + _msg_buffer_len = max_msg_size; + _msg_buffer = new uint8_t[_msg_buffer_len]; + + if (!_msg_buffer) { + PX4_ERR("failed to alloc message buffer"); + return; + } + } + + + if (!_writer.init()) { + PX4_ERR("writer init failed"); + return; + } + + /* debug stats */ + hrt_abstime timer_start = 0; + uint32_t total_bytes = 0; + + px4_register_shutdown_hook(&Logger::request_stop_static); + + const bool disable_boot_logging = get_disable_boot_logging(); + + if ((_log_mode == LogMode::boot_until_disarm || _log_mode == LogMode::boot_until_shutdown) && !disable_boot_logging) { + start_log_file(LogType::Full); + } + + /* init the update timer */ + struct hrt_call timer_call {}; + timer_callback_data_s timer_callback_data; + px4_sem_init(&timer_callback_data.semaphore, 0, 0); + /* timer_semaphore use case is a signal */ + px4_sem_setprotocol(&timer_callback_data.semaphore, SEM_PRIO_NONE); + + int polling_topic_sub = -1; + + if (_polling_topic_meta) { + polling_topic_sub = orb_subscribe(_polling_topic_meta); + + if (polling_topic_sub < 0) { + PX4_ERR("Failed to subscribe (%i)", errno); + } + + } else { + + if (_writer.backend() & LogWriter::BackendFile) { + + const pid_t pid_self = getpid(); + const pthread_t writer_thread = _writer.thread_id_file(); + + // sched_note_start is already called from pthread_create and task_create, + // which means we can expect to find the tasks in system_load.tasks, as required in watchdog_initialize + watchdog_initialize(pid_self, writer_thread, timer_callback_data.watchdog_data); + } + + hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_callback_data); + } + + // check for new subscription data + hrt_abstime next_subscribe_check = 0; + int next_subscribe_topic_index = -1; // this is used to distribute the checks over time + + if (polling_topic_sub >= 0) { + _lockstep_component = px4_lockstep_register_component(); + } + + while (!should_exit()) { + // Start/stop logging (depending on logging mode, by default when arming/disarming) + const bool logging_started = start_stop_logging(); + + if (logging_started) { +#ifdef DBGPRINT + timer_start = hrt_absolute_time(); + total_bytes = 0; +#endif /* DBGPRINT */ + } + + /* check for logging command from MAVLink (start/stop streaming) */ + handle_vehicle_command_update(); + + if (timer_callback_data.watchdog_triggered) { + timer_callback_data.watchdog_triggered = false; + initialize_load_output(PrintLoadReason::Watchdog); + } + + + const hrt_abstime loop_time = hrt_absolute_time(); + + if (_writer.is_started(LogType::Full)) { // mission log only runs when full log is also started + + /* check if we need to output the process load */ + if (_next_load_print != 0 && loop_time >= _next_load_print) { + _next_load_print = 0; + write_load_output(); + + if (_should_stop_file_log) { + _should_stop_file_log = false; + stop_log_file(LogType::Full); + continue; // skip to next loop iteration + } + } + + /* Check if parameters have changed */ + if (!_should_stop_file_log) { // do not record param changes after disarming + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); + + write_changed_parameters(LogType::Full); + } + } + + /* wait for lock on log buffer */ + _writer.lock(); + + for (int sub_idx = 0; sub_idx < _num_subscriptions; ++sub_idx) { + LoggerSubscription &sub = _subscriptions[sub_idx]; + /* if this topic has been updated, copy the new data into the message buffer + * and write a message to the log + */ + const bool try_to_subscribe = (sub_idx == next_subscribe_topic_index); + + if (copy_if_updated(sub_idx, _msg_buffer + sizeof(ulog_message_data_header_s), try_to_subscribe)) { + // each message consists of a header followed by an orb data object + const size_t msg_size = sizeof(ulog_message_data_header_s) + sub.get_topic()->o_size_no_padding; + const uint16_t write_msg_size = static_cast(msg_size - ULOG_MSG_HEADER_LEN); + const uint16_t write_msg_id = sub.msg_id; + + //write one byte after another (necessary because of alignment) + _msg_buffer[0] = (uint8_t)write_msg_size; + _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); + _msg_buffer[2] = static_cast(ULogMessageType::DATA); + _msg_buffer[3] = (uint8_t)write_msg_id; + _msg_buffer[4] = (uint8_t)(write_msg_id >> 8); + + // PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.get_topic()->o_name, sub.get_topic()->o_size, msg_size); + + // full log + if (write_message(LogType::Full, _msg_buffer, msg_size)) { + +#ifdef DBGPRINT + total_bytes += msg_size; +#endif /* DBGPRINT */ + } + + // mission log + if (sub_idx < _num_mission_subs) { + if (_writer.is_started(LogType::Mission)) { + if (_mission_subscriptions[sub_idx].next_write_time < (loop_time / 100000)) { + unsigned delta_time = _mission_subscriptions[sub_idx].min_delta_ms; + + if (delta_time > 0) { + _mission_subscriptions[sub_idx].next_write_time = (loop_time / 100000) + delta_time / 100; + } + + write_message(LogType::Mission, _msg_buffer, msg_size); + } + } + } + } + } + + // check for new logging message(s) + log_message_s log_message; + + if (_log_message_sub.update(&log_message)) { + const char *message = (const char *)log_message.text; + int message_len = strlen(message); + + if (message_len > 0) { + uint16_t write_msg_size = sizeof(ulog_message_logging_s) - sizeof(ulog_message_logging_s::message) + - ULOG_MSG_HEADER_LEN + message_len; + _msg_buffer[0] = (uint8_t)write_msg_size; + _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); + _msg_buffer[2] = static_cast(ULogMessageType::LOGGING); + _msg_buffer[3] = log_message.severity + '0'; + memcpy(_msg_buffer + 4, &log_message.timestamp, sizeof(ulog_message_logging_s::timestamp)); + strncpy((char *)(_msg_buffer + 12), message, sizeof(ulog_message_logging_s::message)); + + write_message(LogType::Full, _msg_buffer, write_msg_size + ULOG_MSG_HEADER_LEN); + } + } + + // Add sync magic + if (loop_time - _last_sync_time > 500_ms) { + uint16_t write_msg_size = static_cast(sizeof(ulog_message_sync_s) - ULOG_MSG_HEADER_LEN); + _msg_buffer[0] = (uint8_t)write_msg_size; + _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); + _msg_buffer[2] = static_cast(ULogMessageType::SYNC); + + // sync byte sequence + _msg_buffer[3] = 0x2F; + _msg_buffer[4] = 0x73; + _msg_buffer[5] = 0x13; + _msg_buffer[6] = 0x20; + _msg_buffer[7] = 0x25; + _msg_buffer[8] = 0x0C; + _msg_buffer[9] = 0xBB; + _msg_buffer[10] = 0x12; + + write_message(LogType::Full, _msg_buffer, write_msg_size + ULOG_MSG_HEADER_LEN); + _last_sync_time = loop_time; + } + + // update buffer statistics + for (int i = 0; i < (int)LogType::Count; ++i) { + if (!_statistics[i].dropout_start && (_writer.get_buffer_fill_count_file((LogType)i) > _statistics[i].high_water)) { + _statistics[i].high_water = _writer.get_buffer_fill_count_file((LogType)i); + } + } + + publish_logger_status(); + + /* release the log buffer */ + _writer.unlock(); + + /* notify the writer thread */ + _writer.notify(); + + /* subscription update */ + if (next_subscribe_topic_index != -1) { + if (++next_subscribe_topic_index >= _num_subscriptions) { + next_subscribe_topic_index = -1; + next_subscribe_check = loop_time + TRY_SUBSCRIBE_INTERVAL; + } + + } else if (loop_time > next_subscribe_check) { + next_subscribe_topic_index = 0; + } + + debug_print_buffer(total_bytes, timer_start); + + } else { // not logging + + // try to subscribe to new topics, even if we don't log, so that: + // - we avoid subscribing to many topics at once, when logging starts + // - we'll get the data immediately once we start logging (no need to wait for the next subscribe timeout) + if (next_subscribe_topic_index != -1) { + if (!_subscriptions[next_subscribe_topic_index].valid()) { + _subscriptions[next_subscribe_topic_index].subscribe(); + } + + if (++next_subscribe_topic_index >= _num_subscriptions) { + next_subscribe_topic_index = -1; + next_subscribe_check = loop_time + TRY_SUBSCRIBE_INTERVAL; + } + + } else if (loop_time > next_subscribe_check) { + next_subscribe_topic_index = 0; + } + } + + update_params(); + + // wait for next loop iteration... + if (polling_topic_sub >= 0) { + px4_lockstep_progress(_lockstep_component); + + px4_pollfd_struct_t fds[1]; + fds[0].fd = polling_topic_sub; + fds[0].events = POLLIN; + int pret = px4_poll(fds, 1, 20); + + if (pret < 0) { + PX4_ERR("poll failed (%i)", pret); + + } else if (pret != 0) { + if (fds[0].revents & POLLIN) { + // need to to an orb_copy so that the next poll will not return immediately + orb_copy(_polling_topic_meta, polling_topic_sub, _msg_buffer); + } + } + + } else { + /* + * We wait on the semaphore, which periodically gets updated by a high-resolution timer. + * The simpler alternative would be: + * usleep(max(300, _log_interval - elapsed_time_since_loop_start)); + * And on linux this is quite accurate as well, but under NuttX it is not accurate, + * because usleep() has only a granularity of CONFIG_MSEC_PER_TICK (=1ms). + */ + while (px4_sem_wait(&timer_callback_data.semaphore) != 0) {} + } + } + + px4_lockstep_unregister_component(_lockstep_component); + + stop_log_file(LogType::Full); + stop_log_file(LogType::Mission); + + hrt_cancel(&timer_call); + px4_sem_destroy(&timer_callback_data.semaphore); + + // stop the writer thread + _writer.thread_stop(); + + if (polling_topic_sub >= 0) { + orb_unsubscribe(polling_topic_sub); + } + + if (_mavlink_log_pub) { + orb_unadvertise(_mavlink_log_pub); + _mavlink_log_pub = nullptr; + } + + px4_unregister_shutdown_hook(&Logger::request_stop_static); +} + +void Logger::debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start) +{ +#ifdef DBGPRINT + double deltat = (double)(hrt_absolute_time() - timer_start) * 1e-6; + + if (deltat > 4.0) { + struct mallinfo alloc_info = mallinfo(); + double throughput = total_bytes / deltat; + PX4_INFO("%8.1f kB/s, %zu highWater, %d dropouts, %5.3f sec max, free heap: %d", + throughput / 1.e3, _statistics[0].high_water, _statistics[0].write_dropouts, + (double)_statistics[0].max_dropout_duration, alloc_info.fordblks); + + _statistics[0].high_water = 0; + _statistics[0].max_dropout_duration = 0.f; + total_bytes = 0; + timer_start = hrt_absolute_time(); + } + +#endif /* DBGPRINT */ +} + +void Logger::publish_logger_status() +{ + if (hrt_elapsed_time(&_logger_status_last) >= 1_s) { + for (int i = 0; i < (int)LogType::Count; ++i) { + + const LogType log_type = static_cast(i); + + if (_writer.is_started(log_type)) { + + const size_t buffer_fill_count_file = _writer.get_buffer_fill_count_file(log_type); + + const float kb_written = _writer.get_total_written_file(log_type) / 1024.0f; + const float seconds = hrt_elapsed_time(&_statistics[i].start_time_file) * 1e-6f; + + logger_status_s status; + status.type = i; + status.backend = _writer.backend(); + status.total_written_kb = kb_written; + status.write_rate_kb_s = kb_written / seconds; + status.dropouts = _statistics[i].write_dropouts; + status.message_gaps = _message_gaps; + status.buffer_used_bytes = buffer_fill_count_file; + status.buffer_size_bytes = _writer.get_buffer_size_file(log_type); + status.num_messages = _num_subscriptions; + status.timestamp = hrt_absolute_time(); + _logger_status_pub[i].publish(status); + } + } + + _logger_status_last = hrt_absolute_time(); + } +} + +bool Logger::get_disable_boot_logging() +{ + if (_param_sdlog_boot_bat.get()) { + battery_status_s battery_status; + uORB::Subscription battery_status_sub{ORB_ID(battery_status)}; + + if (battery_status_sub.copy(&battery_status)) { + if (!battery_status.connected) { + return true; + } + + } else { + PX4_WARN("battery_status not published. Logging anyway"); + } + } + + return false; +} + +bool Logger::start_stop_logging() +{ + bool updated = false; + bool desired_state = false; + + if (_log_mode == LogMode::rc_aux1) { + // aux1-based logging + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + desired_state = (manual_control_setpoint.aux1 > 0.3f); + updated = true; + } + + } else if (_log_mode != LogMode::boot_until_shutdown) { + // arming-based logging + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + + desired_state = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + updated = true; + } + } + + desired_state = desired_state || _manually_logging_override; + + // only start/stop if this is a state transition + if (updated && _prev_state != desired_state) { + _prev_state = desired_state; + + if (desired_state) { + if (_should_stop_file_log) { // happens on quick stop/start toggling + _should_stop_file_log = false; + stop_log_file(LogType::Full); + } + + start_log_file(LogType::Full); + + if ((MissionLogType)_param_sdlog_mission.get() != MissionLogType::Disabled) { + start_log_file(LogType::Mission); + } + + return true; + + } else { + // delayed stop: we measure the process loads and then stop + initialize_load_output(PrintLoadReason::Postflight); + _should_stop_file_log = true; + + if ((MissionLogType)_param_sdlog_mission.get() != MissionLogType::Disabled) { + stop_log_file(LogType::Mission); + } + } + } + + return false; +} + +void Logger::handle_vehicle_command_update() +{ + vehicle_command_s command; + + if (_vehicle_command_sub.update(&command)) { + + if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + + if ((int)(command.param1 + 0.5f) != 0) { + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + + } else if (can_start_mavlink_log()) { + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + start_log_mavlink(); + + } else { + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + } + + } else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) { + stop_log_mavlink(); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + } + } +} + +bool Logger::write_message(LogType type, void *ptr, size_t size) +{ + Statistics &stats = _statistics[(int)type]; + + if (_writer.write_message(type, ptr, size, stats.dropout_start) != -1) { + + if (stats.dropout_start) { + float dropout_duration = (float)(hrt_elapsed_time(&stats.dropout_start) / 1000) / 1.e3f; + + if (dropout_duration > stats.max_dropout_duration) { + stats.max_dropout_duration = dropout_duration; + } + + stats.dropout_start = 0; + } + + return true; + } + + if (!stats.dropout_start) { + stats.dropout_start = hrt_absolute_time(); + ++stats.write_dropouts; + stats.high_water = 0; + } + + return false; +} + +int Logger::create_log_dir(LogType type, tm *tt, char *log_dir, int log_dir_len) +{ + LogFileName &file_name = _file_name[(int)type]; + + /* create dir on sdcard if needed */ + int n = snprintf(log_dir, log_dir_len, "%s/", LOG_ROOT[(int)type]); + + if (n >= log_dir_len) { + PX4_ERR("log path too long"); + return -1; + } + + if (tt) { + strftime(file_name.log_dir, sizeof(LogFileName::log_dir), "%Y-%m-%d", tt); + strncpy(log_dir + n, file_name.log_dir, log_dir_len - n); + int mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret != OK && errno != EEXIST) { + PX4_ERR("failed creating new dir: %s", log_dir); + return -1; + } + + } else { + uint16_t dir_number = file_name.sess_dir_index; + + if (file_name.has_log_dir) { + strncpy(log_dir + n, file_name.log_dir, log_dir_len - n); + } + + /* look for the next dir that does not exist */ + while (!file_name.has_log_dir) { + /* format log dir: e.g. /fs/microsd/log/sess001 */ + int n2 = snprintf(file_name.log_dir, sizeof(LogFileName::log_dir), "sess%03" PRIu16, dir_number); + + if (n2 >= (int)sizeof(LogFileName::log_dir)) { + PX4_ERR("log path too long (%i)", n); + return -1; + } + + strncpy(log_dir + n, file_name.log_dir, log_dir_len - n); + int mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + PX4_DEBUG("log dir created: %s", log_dir); + file_name.has_log_dir = true; + + } else if (errno != EEXIST) { + PX4_ERR("failed creating new dir: %s (%i)", log_dir, errno); + return -1; + } + + /* dir exists already */ + dir_number++; + } + + file_name.has_log_dir = true; + } + + return strlen(log_dir); +} + +int Logger::get_log_file_name(LogType type, char *file_name, size_t file_name_size) +{ + tm tt = {}; + bool time_ok = false; + + if (_log_name_timestamp) { + /* use RTC time for log file naming, e.g. /fs/microsd/log/2014-01-19/19_37_52.ulg */ + time_ok = util::get_log_time(&tt, _param_sdlog_utc_offset.get() * 60, false); + } + + const char *replay_suffix = ""; + + if (_replay_file_name) { + replay_suffix = "_replayed"; + } + + char *log_file_name = _file_name[(int)type].log_file_name; + + if (time_ok) { + int n = create_log_dir(type, &tt, file_name, file_name_size); + + if (n < 0) { + return -1; + } + + char log_file_name_time[16] = ""; + strftime(log_file_name_time, sizeof(log_file_name_time), "%H_%M_%S", &tt); + snprintf(log_file_name, sizeof(LogFileName::log_file_name), "%s%s.ulg", log_file_name_time, replay_suffix); + snprintf(file_name + n, file_name_size - n, "/%s", log_file_name); + + } else { + int n = create_log_dir(type, nullptr, file_name, file_name_size); + + if (n < 0) { + return -1; + } + + uint16_t file_number = 1; // start with file log001 + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/log/sess001/log001.ulg */ + snprintf(log_file_name, sizeof(LogFileName::log_file_name), "log%03" PRIu16 "%s.ulg", file_number, replay_suffix); + snprintf(file_name + n, file_name_size - n, "/%s", log_file_name); + + if (!util::file_exist(file_name)) { + break; + } + + file_number++; + } + + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + return -1; + } + } + + return 0; +} + +void Logger::setReplayFile(const char *file_name) +{ + if (_replay_file_name) { + free(_replay_file_name); + } + + _replay_file_name = strdup(file_name); +} + +void Logger::start_log_file(LogType type) +{ + if (_writer.is_started(type, LogWriter::BackendFile) || (_writer.backend() & LogWriter::BackendFile) == 0) { + return; + } + + if (type == LogType::Full) { + // initialize cpu load as early as possible to get more data + initialize_load_output(PrintLoadReason::Preflight); + } + + PX4_INFO("Start file log (type: %s)", log_type_str(type)); + + char file_name[LOG_DIR_LEN] = ""; + + if (get_log_file_name(type, file_name, sizeof(file_name))) { + PX4_ERR("failed to get log file name"); + return; + } + + if (type == LogType::Full) { + /* print logging path, important to find log file later */ + mavlink_log_info(&_mavlink_log_pub, "[logger] %s", file_name); + } + + _writer.start_log_file(type, file_name); + _writer.select_write_backend(LogWriter::BackendFile); + _writer.set_need_reliable_transfer(true); + write_header(type); + write_version(type); + write_formats(type); + + if (type == LogType::Full) { + write_parameters(type); + write_parameter_defaults(type); + write_perf_data(true); + write_console_output(); + } + + write_all_add_logged_msg(type); + _writer.set_need_reliable_transfer(false); + _writer.unselect_write_backend(); + _writer.notify(); + + if (type == LogType::Full) { + /* reset performance counters to get in-flight min and max values in post flight log */ + perf_reset_all(); + } + + _statistics[(int)type].start_time_file = hrt_absolute_time(); + +} + +void Logger::stop_log_file(LogType type) +{ + if (!_writer.is_started(type, LogWriter::BackendFile)) { + return; + } + + if (type == LogType::Full) { + _writer.set_need_reliable_transfer(true); + write_perf_data(false); + _writer.set_need_reliable_transfer(false); + } + + _writer.stop_log_file(type); +} + +void Logger::start_log_mavlink() +{ + if (!can_start_mavlink_log()) { + return; + } + + // initialize cpu load as early as possible to get more data + initialize_load_output(PrintLoadReason::Preflight); + + PX4_INFO("Start mavlink log"); + + _writer.start_log_mavlink(); + _writer.select_write_backend(LogWriter::BackendMavlink); + _writer.set_need_reliable_transfer(true); + write_header(LogType::Full); + write_version(LogType::Full); + write_formats(LogType::Full); + write_parameters(LogType::Full); + write_parameter_defaults(LogType::Full); + write_perf_data(true); + write_console_output(); + write_all_add_logged_msg(LogType::Full); + _writer.set_need_reliable_transfer(false); + _writer.unselect_write_backend(); + _writer.notify(); +} + +void Logger::stop_log_mavlink() +{ + // don't write perf data since a client does not expect more data after a stop command + PX4_INFO("Stop mavlink log"); + _writer.stop_log_mavlink(); +} + +struct perf_callback_data_t { + Logger *logger; + int counter; + bool preflight; + char *buffer; +}; + +void Logger::perf_iterate_callback(perf_counter_t handle, void *user) +{ + perf_callback_data_t *callback_data = (perf_callback_data_t *)user; + const int buffer_length = 220; + char buffer[buffer_length]; + const char *perf_name; + + perf_print_counter_buffer(buffer, buffer_length, handle); + + if (callback_data->preflight) { + perf_name = "perf_counter_preflight"; + + } else { + perf_name = "perf_counter_postflight"; + } + + callback_data->logger->write_info_multiple(LogType::Full, perf_name, buffer, callback_data->counter != 0); + ++callback_data->counter; +} + +void Logger::write_perf_data(bool preflight) +{ + perf_callback_data_t callback_data = {}; + callback_data.logger = this; + callback_data.counter = 0; + callback_data.preflight = preflight; + + // write the perf counters + perf_iterate_all(perf_iterate_callback, &callback_data); +} + + +void Logger::print_load_callback(void *user) +{ + perf_callback_data_t *callback_data = (perf_callback_data_t *)user; + const char *perf_name; + + if (!callback_data->buffer) { + return; + } + + switch (callback_data->logger->_print_load_reason) { + case PrintLoadReason::Preflight: + perf_name = "perf_top_preflight"; + break; + + case PrintLoadReason::Postflight: + perf_name = "perf_top_postflight"; + break; + + case PrintLoadReason::Watchdog: + perf_name = "perf_top_watchdog"; + break; + + default: + perf_name = "perf_top"; + break; + } + + callback_data->logger->write_info_multiple(LogType::Full, perf_name, callback_data->buffer, + callback_data->counter != 0); + ++callback_data->counter; +} + +void Logger::initialize_load_output(PrintLoadReason reason) +{ + init_print_load(&_load); + _next_load_print = hrt_absolute_time() + 1_s; + _print_load_reason = reason; +} + +void Logger::write_load_output() +{ + if (_print_load_reason == PrintLoadReason::Watchdog) { + PX4_ERR("Writing watchdog data"); // this is just that we see it easily in the log + } + + perf_callback_data_t callback_data = {}; + char buffer[140]; + callback_data.logger = this; + callback_data.counter = 0; + callback_data.buffer = buffer; + _writer.set_need_reliable_transfer(true); + // TODO: maybe we should restrict the output to a selected backend (eg. when file logging is running + // and mavlink log is started, this will be added to the file as well) + print_load_buffer(buffer, sizeof(buffer), print_load_callback, &callback_data, &_load); + cpuload_monitor_stop(); + _writer.set_need_reliable_transfer(false); +} + +void Logger::write_console_output() +{ + const int buffer_length = 220; + char buffer[buffer_length]; + int size = px4_console_buffer_size(); + int offset = -1; + bool first = true; + + while (size > 0) { + int read_size = px4_console_buffer_read(buffer, buffer_length - 1, &offset); + + if (read_size <= 0) { break; } + + buffer[math::min(read_size, size)] = '\0'; + write_info_multiple(LogType::Full, "boot_console_output", buffer, !first); + + size -= read_size; + first = false; + } + +} + +void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats &written_formats, + ulog_message_format_s &msg, int subscription_index, int level) +{ + if (level > 3) { + // precaution: limit recursion level. If we land here it's either a bug or nested topic definitions. In the + // latter case, increase the maximum level. + PX4_ERR("max recursion level reached (%i)", level); + return; + } + + // check if we already wrote the format: either if at a previous _subscriptions index or in written_formats + for (const auto &written_format : written_formats) { + if (written_format == &meta) { + PX4_DEBUG("already added: %s", meta.o_name); + return; + } + } + + for (int i = 0; i < subscription_index; ++i) { + if (_subscriptions[i].get_topic() == &meta) { + PX4_DEBUG("already in _subscriptions: %s", meta.o_name); + return; + } + } + + PX4_DEBUG("writing format for %s", meta.o_name); + + // Write the current format (we don't need to check if we already added it to written_formats) + int format_len = snprintf(msg.format, sizeof(msg.format), "%s:%s", meta.o_name, meta.o_fields); + size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len; + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_message(type, &msg, msg_size); + + if (level > 1 && !written_formats.push_back(&meta)) { + PX4_ERR("Array too small"); + } + + // Now go through the fields and check for nested type usages. + // o_fields looks like this for example: "uint64_t timestamp;uint8_t[5] array;" + const char *fmt = meta.o_fields; + + while (fmt && *fmt) { + // extract the type name + char type_name[64]; + const char *space = strchr(fmt, ' '); + + if (!space) { + PX4_ERR("invalid format %s", fmt); + break; + } + + const char *array_start = strchr(fmt, '['); // check for an array + + int type_length; + + if (array_start && array_start < space) { + type_length = array_start - fmt; + + } else { + type_length = space - fmt; + } + + if (type_length >= (int)sizeof(type_name)) { + PX4_ERR("buf len too small"); + break; + } + + memcpy(type_name, fmt, type_length); + type_name[type_length] = '\0'; + + // ignore built-in types + if (strcmp(type_name, "int8_t") != 0 && + strcmp(type_name, "uint8_t") != 0 && + strcmp(type_name, "int16_t") != 0 && + strcmp(type_name, "uint16_t") != 0 && + strcmp(type_name, "int16_t") != 0 && + strcmp(type_name, "uint16_t") != 0 && + strcmp(type_name, "int32_t") != 0 && + strcmp(type_name, "uint32_t") != 0 && + strcmp(type_name, "int64_t") != 0 && + strcmp(type_name, "uint64_t") != 0 && + strcmp(type_name, "float") != 0 && + strcmp(type_name, "double") != 0 && + strcmp(type_name, "bool") != 0 && + strcmp(type_name, "char") != 0) { + + // find orb meta for type + const orb_metadata *const *topics = orb_get_topics(); + const orb_metadata *found_topic = nullptr; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(topics[i]->o_name, type_name) == 0) { + found_topic = topics[i]; + } + } + + if (found_topic) { + + write_format(type, *found_topic, written_formats, msg, subscription_index, level + 1); + + } else { + PX4_ERR("No definition for topic %s found", fmt); + } + } + + fmt = strchr(fmt, ';'); + + if (fmt) { ++fmt; } + } +} + +void Logger::write_formats(LogType type) +{ + _writer.lock(); + + // both of these are large and thus we need to be careful in terms of stack size requirements + ulog_message_format_s msg; + WrittenFormats written_formats; + + // write all subscribed formats + int sub_count = _num_subscriptions; + + if (type == LogType::Mission) { + sub_count = _num_mission_subs; + } + + for (int i = 0; i < sub_count; ++i) { + const LoggerSubscription &sub = _subscriptions[i]; + write_format(type, *sub.get_topic(), written_formats, msg, i); + } + + _writer.unlock(); +} + +void Logger::write_all_add_logged_msg(LogType type) +{ + _writer.lock(); + + int sub_count = _num_subscriptions; + + if (type == LogType::Mission) { + sub_count = _num_mission_subs; + } + + bool added_subscriptions = false; + + for (int i = 0; i < sub_count; ++i) { + LoggerSubscription &sub = _subscriptions[i]; + + if (sub.valid()) { + write_add_logged_msg(type, sub); + added_subscriptions = true; + } + } + + _writer.unlock(); + + if (!added_subscriptions) { + PX4_ERR("No subscriptions added"); // this results in invalid log files + } +} + +void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription) +{ + ulog_message_add_logged_s msg; + + if (subscription.msg_id == MSG_ID_INVALID) { + if (_next_topic_id == MSG_ID_INVALID) { + // if we land here an uint8 is too small -> switch to uint16 + PX4_ERR("limit for _next_topic_id reached"); + return; + } + + subscription.msg_id = _next_topic_id++; + } + + msg.msg_id = subscription.msg_id; + msg.multi_id = subscription.get_instance(); + + int message_name_len = strlen(subscription.get_topic()->o_name); + + memcpy(msg.message_name, subscription.get_topic()->o_name, message_name_len); + + size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len; + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + bool prev_reliable = _writer.need_reliable_transfer(); + _writer.set_need_reliable_transfer(true); + write_message(type, &msg, msg_size); + _writer.set_need_reliable_transfer(prev_reliable); +} + +void Logger::write_info(LogType type, const char *name, const char *value) +{ + _writer.lock(); + ulog_message_info_header_s msg = {}; + uint8_t *buffer = reinterpret_cast(&msg); + msg.msg_type = static_cast(ULogMessageType::INFO); + + /* construct format key (type and name) */ + size_t vlen = strlen(value); + msg.key_len = snprintf(msg.key, sizeof(msg.key), "char[%zu] %s", vlen, name); + size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len; + + /* copy string value directly to buffer */ + if (vlen < (sizeof(msg) - msg_size)) { + memcpy(&buffer[msg_size], value, vlen); + msg_size += vlen; + + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_message(type, buffer, msg_size); + } + + _writer.unlock(); +} + +void Logger::write_info_multiple(LogType type, const char *name, const char *value, bool is_continued) +{ + _writer.lock(); + ulog_message_info_multiple_header_s msg; + uint8_t *buffer = reinterpret_cast(&msg); + msg.msg_type = static_cast(ULogMessageType::INFO_MULTIPLE); + msg.is_continued = is_continued; + + /* construct format key (type and name) */ + size_t vlen = strlen(value); + msg.key_len = snprintf(msg.key, sizeof(msg.key), "char[%zu] %s", vlen, name); + size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len; + + /* copy string value directly to buffer */ + if (vlen < (sizeof(msg) - msg_size)) { + memcpy(&buffer[msg_size], value, vlen); + msg_size += vlen; + + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_message(type, buffer, msg_size); + + } else { + PX4_ERR("info_multiple str too long (%" PRIu8 "), key=%s", msg.key_len, msg.key); + } + + _writer.unlock(); +} + +void Logger::write_info(LogType type, const char *name, int32_t value) +{ + write_info_template(type, name, value, "int32_t"); +} + +void Logger::write_info(LogType type, const char *name, uint32_t value) +{ + write_info_template(type, name, value, "uint32_t"); +} + + +template +void Logger::write_info_template(LogType type, const char *name, T value, const char *type_str) +{ + _writer.lock(); + ulog_message_info_header_s msg = {}; + uint8_t *buffer = reinterpret_cast(&msg); + msg.msg_type = static_cast(ULogMessageType::INFO); + + /* construct format key (type and name) */ + msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, name); + size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len; + + /* copy string value directly to buffer */ + memcpy(&buffer[msg_size], &value, sizeof(T)); + msg_size += sizeof(T); + + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_message(type, buffer, msg_size); + + _writer.unlock(); +} + +void Logger::write_header(LogType type) +{ + ulog_file_header_s header = {}; + header.magic[0] = 'U'; + header.magic[1] = 'L'; + header.magic[2] = 'o'; + header.magic[3] = 'g'; + header.magic[4] = 0x01; + header.magic[5] = 0x12; + header.magic[6] = 0x35; + header.magic[7] = 0x01; //file version 1 + header.timestamp = hrt_absolute_time(); + _writer.lock(); + write_message(type, &header, sizeof(header)); + + // write the Flags message: this MUST be written right after the ulog header + ulog_message_flag_bits_s flag_bits{}; + + flag_bits.compat_flags[0] = ULOG_COMPAT_FLAG0_DEFAULT_PARAMETERS_MASK; + + flag_bits.msg_size = sizeof(flag_bits) - ULOG_MSG_HEADER_LEN; + flag_bits.msg_type = static_cast(ULogMessageType::FLAG_BITS); + + write_message(type, &flag_bits, sizeof(flag_bits)); + + _writer.unlock(); +} + +void Logger::write_version(LogType type) +{ + write_info(type, "ver_sw", px4_firmware_version_string()); + write_info(type, "ver_sw_release", px4_firmware_version()); + uint32_t vendor_version = px4_firmware_vendor_version(); + + if (vendor_version > 0) { + write_info(type, "ver_vendor_sw_release", vendor_version); + } + + write_info(type, "ver_hw", px4_board_name()); + const char *board_sub_type = px4_board_sub_type(); + + if (board_sub_type && board_sub_type[0]) { + write_info(type, "ver_hw_subtype", board_sub_type); + } + + write_info(type, "sys_name", "PX4"); + write_info(type, "sys_os_name", px4_os_name()); + const char *os_version = px4_os_version_string(); + + const char *git_branch = px4_firmware_git_branch(); + + if (git_branch && git_branch[0]) { + write_info(type, "ver_sw_branch", git_branch); + } + + if (os_version) { + write_info(type, "sys_os_ver", os_version); + } + + const char *oem_version = px4_firmware_oem_version_string(); + + if (oem_version && oem_version[0]) { + write_info(type, "ver_oem", oem_version); + } + + + write_info(type, "sys_os_ver_release", px4_os_version()); + write_info(type, "sys_toolchain", px4_toolchain_name()); + write_info(type, "sys_toolchain_ver", px4_toolchain_version()); + + const char *ecl_version = px4_ecl_lib_version_string(); + + if (ecl_version && ecl_version[0]) { + write_info(type, "sys_lib_ecl_ver", ecl_version); + } + + char revision = 'U'; + const char *chip_name = nullptr; + + if (board_mcu_version(&revision, &chip_name, nullptr) >= 0) { + char mcu_ver[64]; + snprintf(mcu_ver, sizeof(mcu_ver), "%s, rev. %c", chip_name, revision); + write_info(type, "sys_mcu", mcu_ver); + } + + // data versioning: increase this on every larger data change (format/semantic) + // 1: switch to FIFO drivers (disabled on-chip DLPF) + write_info(type, "ver_data_format", static_cast(1)); + +#ifndef BOARD_HAS_NO_UUID + + /* write the UUID if enabled */ + if (_param_sdlog_uuid.get() == 1) { + char px4_uuid_string[PX4_GUID_FORMAT_SIZE]; + board_get_px4_guid_formated(px4_uuid_string, sizeof(px4_uuid_string)); + write_info(type, "sys_uuid", px4_uuid_string); + } + +#endif /* BOARD_HAS_NO_UUID */ + + write_info(type, "time_ref_utc", _param_sdlog_utc_offset.get() * 60); + + if (_replay_file_name) { + write_info(type, "replay", _replay_file_name); + } + + if (type == LogType::Mission) { + write_info(type, "log_type", "mission"); + } +} + +void Logger::write_parameter_defaults(LogType type) +{ + _writer.lock(); + ulog_message_parameter_default_header_s msg = {}; + uint8_t *buffer = reinterpret_cast(&msg); + + msg.msg_type = static_cast(ULogMessageType::PARAMETER_DEFAULT); + int param_idx = 0; + param_t param = 0; + + do { + // skip over all parameters which are not used + do { + param = param_for_index(param_idx); + ++param_idx; + } while (param != PARAM_INVALID && !param_used(param)); + + // save parameters which are valid AND used AND not volatile + if (param != PARAM_INVALID) { + + if (param_is_volatile(param)) { + continue; + } + + // get parameter type and size + const char *type_str; + param_type_t ptype = param_type(param); + size_t value_size = 0; + + uint8_t default_value[math::max(sizeof(float), sizeof(int32_t))]; + uint8_t system_default_value[sizeof(default_value)]; + uint8_t value[sizeof(default_value)]; + + switch (ptype) { + case PARAM_TYPE_INT32: + type_str = "int32_t"; + value_size = sizeof(int32_t); + param_get(param, (int32_t *)&value); + break; + + case PARAM_TYPE_FLOAT: + type_str = "float"; + value_size = sizeof(float); + param_get(param, (float *)&value); + break; + + default: + continue; + } + + // format parameter key (type and name) + msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param)); + size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len; + + if (param_get_default_value(param, &default_value) != 0) { + continue; + } + + if (param_get_system_default_value(param, &system_default_value) != 0) { + continue; + } + + msg_size += value_size; + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + // write the system/airframe default if different from the current value + if (memcmp(&system_default_value, &default_value, value_size) == 0) { + // if the system and airframe defaults are equal, we can combine them + if (memcmp(&value, &default_value, value_size) != 0) { + memcpy(&buffer[msg_size - value_size], default_value, value_size); + msg.default_types = ulog_parameter_default_type_t::current_setup | ulog_parameter_default_type_t::system; + write_message(type, buffer, msg_size); + } + + } else { + if (memcmp(&value, &default_value, value_size) != 0) { + memcpy(&buffer[msg_size - value_size], default_value, value_size); + msg.default_types = ulog_parameter_default_type_t::current_setup; + write_message(type, buffer, msg_size); + } + + if (memcmp(&value, &system_default_value, value_size) != 0) { + memcpy(&buffer[msg_size - value_size], system_default_value, value_size); + msg.default_types = ulog_parameter_default_type_t::system; + write_message(type, buffer, msg_size); + } + } + } + } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); + + _writer.unlock(); + _writer.notify(); +} + +void Logger::write_parameters(LogType type) +{ + _writer.lock(); + ulog_message_parameter_header_s msg = {}; + uint8_t *buffer = reinterpret_cast(&msg); + + msg.msg_type = static_cast(ULogMessageType::PARAMETER); + int param_idx = 0; + param_t param = 0; + + do { + // skip over all parameters which are not used + do { + param = param_for_index(param_idx); + ++param_idx; + } while (param != PARAM_INVALID && !param_used(param)); + + // save parameters which are valid AND used + if (param != PARAM_INVALID) { + // get parameter type and size + const char *type_str; + param_type_t ptype = param_type(param); + size_t value_size = 0; + + switch (ptype) { + case PARAM_TYPE_INT32: + type_str = "int32_t"; + value_size = sizeof(int32_t); + break; + + case PARAM_TYPE_FLOAT: + type_str = "float"; + value_size = sizeof(float); + break; + + default: + continue; + } + + // format parameter key (type and name) + msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param)); + size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len; + + // copy parameter value directly to buffer + switch (ptype) { + case PARAM_TYPE_INT32: + param_get(param, (int32_t *)&buffer[msg_size]); + break; + + case PARAM_TYPE_FLOAT: + param_get(param, (float *)&buffer[msg_size]); + break; + + default: + continue; + } + + msg_size += value_size; + + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_message(type, buffer, msg_size); + } + } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); + + _writer.unlock(); + _writer.notify(); +} + +void Logger::write_changed_parameters(LogType type) +{ + _writer.lock(); + ulog_message_parameter_header_s msg = {}; + uint8_t *buffer = reinterpret_cast(&msg); + + msg.msg_type = static_cast(ULogMessageType::PARAMETER); + int param_idx = 0; + param_t param = 0; + + do { + // skip over all parameters which are not used + do { + param = param_for_index(param_idx); + ++param_idx; + } while (param != PARAM_INVALID && !param_used(param)); + + // log parameters which are valid AND used AND unsaved + if ((param != PARAM_INVALID) && param_value_unsaved(param)) { + + // get parameter type and size + const char *type_str; + param_type_t ptype = param_type(param); + size_t value_size = 0; + + switch (ptype) { + case PARAM_TYPE_INT32: + type_str = "int32_t"; + value_size = sizeof(int32_t); + break; + + case PARAM_TYPE_FLOAT: + type_str = "float"; + value_size = sizeof(float); + break; + + default: + continue; + } + + // format parameter key (type and name) + msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param)); + size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len; + + // copy parameter value directly to buffer + switch (ptype) { + case PARAM_TYPE_INT32: + param_get(param, (int32_t *)&buffer[msg_size]); + break; + + case PARAM_TYPE_FLOAT: + param_get(param, (float *)&buffer[msg_size]); + break; + + default: + continue; + } + + msg_size += value_size; + + // msg_size is now 1 (msg_type) + 2 (msg_size) + 1 (key_len) + key_len + value_size + msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; + + write_message(type, buffer, msg_size); + } + } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); + + _writer.unlock(); + _writer.notify(); +} + +void Logger::ack_vehicle_command(vehicle_command_s *cmd, uint32_t result) +{ + vehicle_command_ack_s vehicle_command_ack = {}; + vehicle_command_ack.timestamp = hrt_absolute_time(); + vehicle_command_ack.command = cmd->command; + vehicle_command_ack.result = (uint8_t)result; + vehicle_command_ack.target_system = cmd->source_system; + vehicle_command_ack.target_component = cmd->source_component; + + uORB::Publication cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); +} + +int Logger::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +System logger which logs a configurable set of uORB topics and system printf messages +(`PX4_WARN` and `PX4_ERR`) to ULog files. These can be used for system and flight performance evaluation, +tuning, replay and crash analysis. + +It supports 2 backends: +- Files: write ULog files to the file system (SD card) +- MAVLink: stream ULog data via MAVLink to a client (the client must support this) + +Both backends can be enabled and used at the same time. + +The file backend supports 2 types of log files: full (the normal log) and a mission +log. The mission log is a reduced ulog file and can be used for example for geotagging or +vehicle management. It can be enabled and configured via SDLOG_MISSION parameter. +The normal log is always a superset of the mission log. + +### Implementation +The implementation uses two threads: +- The main thread, running at a fixed rate (or polling on a topic if started with -p) and checking for + data updates +- The writer thread, writing data to the file + +In between there is a write buffer with configurable size (and another fixed-size buffer for +the mission log). It should be large to avoid dropouts. + +### Examples +Typical usage to start logging immediately: +$ logger start -e -t + +Or if already running: +$ logger on +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("logger", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('m', "all", "file|mavlink|all", "Backend mode", true); + PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable/disable logging via Aux1 RC channel", true); + PRINT_MODULE_USAGE_PARAM_FLAG('e', "Enable logging right after start until disarm (otherwise only when armed)", true); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Log until shutdown (implies -e)", true); + PRINT_MODULE_USAGE_PARAM_FLAG('t', "Use date/time for naming log directories and files", true); + PRINT_MODULE_USAGE_PARAM_INT('r', 280, 0, 8000, "Log rate in Hz, 0 means unlimited rate", true); + PRINT_MODULE_USAGE_PARAM_INT('b', 12, 4, 10000, "Log buffer size in KiB", true); + PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "", + "Poll on a topic instead of running with fixed rate (Log rate and topic intervals are ignored if this is set)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("on", "start logging now, override arming (logger must be running)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("off", "stop logging now, override arming (logger must be running)"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +} // namespace logger +} // namespace px4 diff --git a/src/prometheus_px4/src/modules/logger/logger.h b/src/prometheus_px4/src/modules/logger/logger.h new file mode 100644 index 0000000..f73497b --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/logger.h @@ -0,0 +1,371 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include "log_writer.h" +#include "messages.h" +#include +#include "util.h" +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern "C" __EXPORT int logger_main(int argc, char *argv[]); + +using namespace time_literals; + +static constexpr hrt_abstime TRY_SUBSCRIBE_INTERVAL{20_ms}; // interval in microseconds at which we try to subscribe to a topic +// if we haven't succeeded before + +namespace px4 +{ +namespace logger +{ + +static constexpr uint8_t MSG_ID_INVALID = UINT8_MAX; + +struct LoggerSubscription : public uORB::SubscriptionInterval { + LoggerSubscription() = default; + + LoggerSubscription(ORB_ID id, uint32_t interval_ms = 0, uint8_t instance = 0) : + uORB::SubscriptionInterval(id, interval_ms * 1000, instance) + {} + + uint8_t msg_id{MSG_ID_INVALID}; +}; + +class Logger : public ModuleBase, public ModuleParams +{ +public: + enum class LogMode { + while_armed = 0, + boot_until_disarm, + boot_until_shutdown, + rc_aux1 + }; + + Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, + LogMode log_mode, bool log_name_timestamp); + + ~Logger(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static Logger *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; + + /** + * Tell the logger that we're in replay mode. This must be called + * before starting the logger. + * @param file_name file name of the used log replay file. Will be copied. + */ + void setReplayFile(const char *file_name); + + /** + * request the logger thread to stop (this method does not block). + * @return true if the logger is stopped, false if (still) running + */ + static bool request_stop_static(); + + void print_statistics(LogType type); + + void set_arm_override(bool override) { _manually_logging_override = override; } + +private: + + enum class PrintLoadReason { + Preflight, + Postflight, + Watchdog + }; + + static constexpr int MAX_MISSION_TOPICS_NUM = 5; /**< Maximum number of mission topics */ + static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ + static constexpr const char *LOG_ROOT[(int)LogType::Count] = { + PX4_STORAGEDIR "/log", + PX4_STORAGEDIR "/mission_log" + }; + + struct LogFileName { + char log_dir[12]; ///< e.g. "2018-01-01" or "sess001" + int sess_dir_index{1}; ///< search starting index for 'sess' directory name + char log_file_name[31]; ///< e.g. "log001.ulg" or "12_09_00_replayed.ulg" + bool has_log_dir{false}; + }; + + struct Statistics { + hrt_abstime start_time_file{0}; ///< Time when logging started, file backend (not the logger thread) + hrt_abstime dropout_start{0}; ///< start of current dropout (0 = no dropout) + float max_dropout_duration{0.0f}; ///< max duration of dropout [s] + size_t write_dropouts{0}; ///< failed buffer writes due to buffer overflow + size_t high_water{0}; ///< maximum used write buffer + }; + + struct MissionSubscription { + unsigned min_delta_ms{0}; ///< minimum time between 2 topic writes [ms] + unsigned next_write_time{0}; ///< next time to write in 0.1 seconds + }; + + /** + * @brief Updates and checks for updated uORB parameters. + */ + void update_params(); + + /** + * Write an ADD_LOGGED_MSG to the log for a all current subscriptions and instances + */ + void write_all_add_logged_msg(LogType type); + + /** + * Write an ADD_LOGGED_MSG to the log for a given subscription and instance. + * _writer.lock() must be held when calling this. + */ + void write_add_logged_msg(LogType type, LoggerSubscription &subscription); + + /** + * Create logging directory + * @param type + * @param tt if not null, use it for the directory name + * @param log_dir returned log directory path + * @param log_dir_len log_dir buffer length + * @return string length of log_dir (excluding terminating null-char), <0 on error + */ + int create_log_dir(LogType type, tm *tt, char *log_dir, int log_dir_len); + + /** + * Get log file name with directory (create it if necessary) + */ + int get_log_file_name(LogType type, char *file_name, size_t file_name_size); + + void start_log_file(LogType type); + + void stop_log_file(LogType type); + + void start_log_mavlink(); + + void stop_log_mavlink(); + + /** check if mavlink logging can be started */ + bool can_start_mavlink_log() const + { + return !_writer.is_started(LogType::Full, LogWriter::BackendMavlink) + && (_writer.backend() & LogWriter::BackendMavlink) != 0; + } + + /** get the configured backend as string */ + const char *configured_backend_mode() const; + + /** + * write the file header with file magic and timestamp. + */ + void write_header(LogType type); + + /// Array to store written formats for nested definitions (only) + using WrittenFormats = Array < const orb_metadata *, 20 >; + + void write_format(LogType type, const orb_metadata &meta, WrittenFormats &written_formats, ulog_message_format_s &msg, + int subscription_index, int level = 1); + void write_formats(LogType type); + + /** + * write performance counters + * @param preflight preflight if true, postflight otherwise + */ + void write_perf_data(bool preflight); + + /** + * write bootup console output + */ + void write_console_output(); + + /** + * callback to write the performance counters + */ + static void perf_iterate_callback(perf_counter_t handle, void *user); + + /** + * callback for print_load_buffer() to print the process load + */ + static void print_load_callback(void *user); + + void write_version(LogType type); + + void write_info(LogType type, const char *name, const char *value); + void write_info_multiple(LogType type, const char *name, const char *value, bool is_continued); + void write_info(LogType type, const char *name, int32_t value); + void write_info(LogType type, const char *name, uint32_t value); + + /** generic common template method for write_info variants */ + template + void write_info_template(LogType type, const char *name, T value, const char *type_str); + + void write_parameters(LogType type); + void write_parameter_defaults(LogType type); + + void write_changed_parameters(LogType type); + + inline bool copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe); + + /** + * Write exactly one ulog message to the logger and handle dropouts. + * Must be called with _writer.lock() held. + * @return true if data written, false otherwise (on overflow) + */ + bool write_message(LogType type, void *ptr, size_t size); + + /** + * Add topic subscriptions from SD file if it exists, otherwise add topics based on the configured profile. + * This must be called before start_log() (because it does not write an ADD_LOGGED_MSG message). + * @return true on success + */ + bool initialize_topics(); + + /** + * Determines if log-from-boot should be disabled, based on the value of SDLOG_BOOT_BAT and the battery status. + * @return true if log-from-boot should be disabled + */ + bool get_disable_boot_logging(); + + /** + * check current arming state or aux channel and start/stop logging if state changed and according to + * configured params. + * @return true if log started + */ + bool start_stop_logging(); + + void handle_vehicle_command_update(); + void ack_vehicle_command(vehicle_command_s *cmd, uint32_t result); + + /** + * initialize the output for the process load, so that ~1 second later it will be written to the log + */ + void initialize_load_output(PrintLoadReason reason); + + /** + * write the process load, which was previously initialized with initialize_load_output() + */ + void write_load_output(); + + /** + * Regularly print the buffer fill state (only if DBGPRINT is set) + * @param total_bytes total written bytes (to the full file), will be reset on each print + * @param timer_start time since last print + */ + inline void debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start); + + void publish_logger_status(); + + uint8_t *_msg_buffer{nullptr}; + int _msg_buffer_len{0}; + + LogFileName _file_name[(int)LogType::Count]; + + bool _prev_state{false}; ///< previous state depending on logging mode (arming or aux1 state) + bool _manually_logging_override{false}; + + Statistics _statistics[(int)LogType::Count]; + hrt_abstime _last_sync_time{0}; ///< last time a sync msg was sent + + LogMode _log_mode; + const bool _log_name_timestamp; + + LoggerSubscription *_subscriptions{nullptr}; ///< all subscriptions for full & mission log (in front) + int _num_subscriptions{0}; + MissionSubscription _mission_subscriptions[MAX_MISSION_TOPICS_NUM] {}; ///< additional data for mission subscriptions + int _num_mission_subs{0}; + + LogWriter _writer; + uint32_t _log_interval{0}; + const orb_metadata *_polling_topic_meta{nullptr}; ///< if non-null, poll on this topic instead of sleeping + orb_advert_t _mavlink_log_pub{nullptr}; + uint8_t _next_topic_id{0}; ///< id of next subscribed ulog topic + char *_replay_file_name{nullptr}; + bool _should_stop_file_log{false}; /**< if true _next_load_print is set and file logging + will be stopped after load printing (for the full log) */ + print_load_s _load{}; ///< process load data + hrt_abstime _next_load_print{0}; ///< timestamp when to print the process load + PrintLoadReason _print_load_reason {PrintLoadReason::Preflight}; + + uORB::PublicationMulti _logger_status_pub[(int)LogType::Count] { ORB_ID(logger_status), ORB_ID(logger_status) }; + + hrt_abstime _logger_status_last {0}; + int _lockstep_component{-1}; + + uint32_t _message_gaps{0}; + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sdlog_utc_offset, + (ParamInt) _param_sdlog_dirs_max, + (ParamInt) _param_sdlog_profile, + (ParamInt) _param_sdlog_mission, + (ParamBool) _param_sdlog_boot_bat, + (ParamBool) _param_sdlog_uuid + ) +}; + +} //namespace logger +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/logger/messages.h b/src/prometheus_px4/src/modules/logger/messages.h new file mode 100644 index 0000000..c6512db --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/messages.h @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include + +enum class ULogMessageType : uint8_t { + FORMAT = 'F', + DATA = 'D', + INFO = 'I', + INFO_MULTIPLE = 'M', + PARAMETER = 'P', + PARAMETER_DEFAULT = 'Q', + ADD_LOGGED_MSG = 'A', + REMOVE_LOGGED_MSG = 'R', + SYNC = 'S', + DROPOUT = 'O', + LOGGING = 'L', + LOGGING_TAGGED = 'C', + FLAG_BITS = 'B', +}; + + +/* declare message data structs with byte alignment (no padding) */ +#pragma pack(push, 1) + +/** first bytes of the file */ +struct ulog_file_header_s { + uint8_t magic[8]; + uint64_t timestamp; +}; + +#define ULOG_MSG_HEADER_LEN 3 //accounts for msg_size and msg_type +struct ulog_message_header_s { + uint16_t msg_size; + uint8_t msg_type; +}; + +struct ulog_message_format_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::FORMAT); + + char format[1500]; +}; + +struct ulog_message_add_logged_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::ADD_LOGGED_MSG); + + uint8_t multi_id; + uint16_t msg_id; + char message_name[255]; +}; + +struct ulog_message_remove_logged_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::REMOVE_LOGGED_MSG); + + uint16_t msg_id; +}; + +struct ulog_message_sync_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::SYNC); + + uint8_t sync_magic[8]; +}; + +struct ulog_message_dropout_s { + uint16_t msg_size = sizeof(uint16_t); //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::DROPOUT); + + uint16_t duration; //in ms +}; + +struct ulog_message_data_header_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::DATA); + + uint16_t msg_id; +}; + +struct ulog_message_info_header_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::INFO); + + uint8_t key_len; + char key[255]; +}; + +struct ulog_message_info_multiple_header_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::INFO_MULTIPLE); + + uint8_t is_continued; ///< can be used for arrays: set to 1, if this message is part of the previous with the same key + uint8_t key_len; + char key[255]; +}; + +struct ulog_message_logging_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::LOGGING); + + uint8_t log_level; //same levels as in the linux kernel + uint64_t timestamp; + char message[128]; //defines the maximum length of a logged message string +}; + +struct ulog_message_logging_tagged_s { + uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN + uint8_t msg_type = static_cast(ULogMessageType::LOGGING_TAGGED); + + uint8_t log_level; //same levels as in the linux kernel + uint16_t tag; + uint64_t timestamp; + char message[128]; //defines the maximum length of a logged message string +}; + +struct ulog_message_parameter_header_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(ULogMessageType::PARAMETER); + + uint8_t key_len; + char key[255]; +}; + +enum class ulog_parameter_default_type_t : uint8_t { + system = (1 << 0), + current_setup = (1 << 1) //airframe default +}; + +inline ulog_parameter_default_type_t operator|(ulog_parameter_default_type_t a, ulog_parameter_default_type_t b) +{ + return static_cast(static_cast(a) | static_cast(b)); +} + +struct ulog_message_parameter_default_header_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(ULogMessageType::PARAMETER_DEFAULT); + + ulog_parameter_default_type_t default_types; + uint8_t key_len; + char key[255]; +}; + + +#define ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK (1<<0) + +#define ULOG_COMPAT_FLAG0_DEFAULT_PARAMETERS_MASK (1<<0) + +struct ulog_message_flag_bits_s { + uint16_t msg_size; + uint8_t msg_type = static_cast(ULogMessageType::FLAG_BITS); + + uint8_t compat_flags[8]; + uint8_t incompat_flags[8]; ///< @see ULOG_INCOMPAT_FLAG_* + uint64_t appended_offsets[3]; ///< file offset(s) for appended data if ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK is set +}; + +#pragma pack(pop) diff --git a/src/prometheus_px4/src/modules/logger/params.c b/src/prometheus_px4/src/modules/logger/params.c new file mode 100644 index 0000000..8220aa8 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/params.c @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * UTC offset (unit: min) + * + * the difference in hours and minutes from Coordinated + * Universal Time (UTC) for a your place and date. + * + * for example, In case of South Korea(UTC+09:00), + * UTC offset is 540 min (9*60) + * + * refer to https://en.wikipedia.org/wiki/List_of_UTC_time_offsets + * + * @unit min + * @min -1000 + * @max 1000 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0); + +/** + * Logging Mode + * + * Determines when to start and stop logging. By default, logging is started + * when arming the system, and stopped when disarming. + * + * @value -1 disabled + * @value 0 when armed until disarm (default) + * @value 1 from boot until disarm + * @value 2 from boot until shutdown + * @value 3 depending on AUX1 RC channel + * + * @reboot_required true + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_MODE, 0); + +/** + * Battery-only Logging + * + * When enabled, logging will not start from boot if battery power is not detected + * (e.g. powered via USB on a test bench). This prevents extraneous flight logs from + * being created during bench testing. + * + * Note that this only applies to log-from-boot modes. This has no effect on arm-based + * modes. + * + * @boolean + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_BOOT_BAT, 0); + +/** + * Mission Log + * + * If enabled, a small additional "mission" log file will be written to the SD card. + * The log contains just those messages that are useful for tasks like + * generating flight statistics and geotagging. + * + * The different modes can be used to further reduce the logged data + * (and thus the log file size). For example, choose geotagging mode to + * only log data required for geotagging. + + * Note that the normal/full log is still created, and contains all + * the data in the mission log (and more). + * + * @value 0 Disabled + * @value 1 All mission messages + * @value 2 Geotagging messages + * + * @reboot_required true + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_MISSION, 0); + +/** + * Logging topic profile (integer bitmask). + * + * This integer bitmask controls the set and rates of logged topics. + * The default allows for general log analysis while keeping the + * log file size reasonably small. + * + * Enabling multiple sets leads to higher bandwidth requirements and larger log + * files. + * + * Set bits true to enable: + * 0 : Default set (used for general log analysis) + * 1 : Full rate estimator (EKF2) replay topics + * 2 : Topics for thermal calibration (high rate raw IMU and Baro sensor data) + * 3 : Topics for system identification (high rate actuator control and IMU data) + * 4 : Full rates for analysis of fast maneuvers (RC, attitude, rates and actuators) + * 5 : Debugging topics (debug_*.msg topics, for custom code) + * 6 : Topics for sensor comparison (low rate raw IMU, Baro and Magnetomer data) + * 7 : Topics for computer vision and collision avoidance + * 8 : Raw FIFO high-rate IMU (Gyro) + * 9 : Raw FIFO high-rate IMU (Accel) + * + * @min 0 + * @max 1023 + * @bit 0 Default set (general log analysis) + * @bit 1 Estimator replay (EKF2) + * @bit 2 Thermal calibration + * @bit 3 System identification + * @bit 4 High rate + * @bit 5 Debug + * @bit 6 Sensor comparison + * @bit 7 Computer Vision and Avoidance + * @bit 8 Raw FIFO high-rate IMU (Gyro) + * @bit 9 Raw FIFO high-rate IMU (Accel) + * @reboot_required true + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_PROFILE, 1); + +/** + * Maximum number of log directories to keep + * + * If there are more log directories than this value, + * the system will delete the oldest directories during startup. + * + * In addition, the system will delete old logs if there is not enough free space left. + * The minimum amount is 300 MB. + * + * If this is set to 0, old directories will only be removed if the free space falls below + * the minimum. + * + * Note: this does not apply to mission log files. + * + * @min 0 + * @max 1000 + * @reboot_required true + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_DIRS_MAX, 0); + +/** + * Log UUID + * + * If set to 1, add an ID to the log, which uniquely identifies the vehicle + * + * @boolean + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_UUID, 1); diff --git a/src/prometheus_px4/src/modules/logger/util.cpp b/src/prometheus_px4/src/modules/logger/util.cpp new file mode 100644 index 0000000..799df12 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/util.cpp @@ -0,0 +1,292 @@ +/**************************************************************************** + * + * 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 "util.h" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#if defined(__PX4_DARWIN) +#include +#include +#else +#include +#endif + +#define GPS_EPOCH_SECS ((time_t)1234567890ULL) + +typedef decltype(statfs::f_bavail) px4_statfs_buf_f_bavail_t; + +namespace px4 +{ +namespace logger +{ +namespace util +{ + +bool file_exist(const char *filename) +{ + struct stat buffer; + return stat(filename, &buffer) == 0; +} + +bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) +{ + uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + + time_t utc_time_sec; + bool use_clock_time = true; + + /* Get the latest GPS publication */ + vehicle_gps_position_s gps_pos; + + if (vehicle_gps_position_sub.copy(&gps_pos)) { + utc_time_sec = gps_pos.time_utc_usec / 1e6; + + if (gps_pos.fix_type >= 2 && utc_time_sec >= GPS_EPOCH_SECS) { + use_clock_time = false; + } + } + + if (use_clock_time) { + /* take clock time if there's no fix (yet) */ + struct timespec ts = {}; + px4_clock_gettime(CLOCK_REALTIME, &ts); + utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + + if (utc_time_sec < GPS_EPOCH_SECS) { + return false; + } + } + + /* strip the time elapsed since boot */ + if (boot_time) { + utc_time_sec -= hrt_absolute_time() / 1e6; + } + + /* apply utc offset */ + utc_time_sec += utc_offset_sec; + + return gmtime_r(&utc_time_sec, tt) != nullptr; +} + +int check_free_space(const char *log_root_dir, int32_t max_log_dirs_to_keep, orb_advert_t &mavlink_log_pub, + int &sess_dir_index) +{ + struct statfs statfs_buf; + + if (max_log_dirs_to_keep == 0) { + max_log_dirs_to_keep = INT32_MAX; + } + + // remove old logs if the free space falls below a threshold + do { + if (statfs(log_root_dir, &statfs_buf) != 0) { + return PX4_ERROR; + } + + DIR *dp = opendir(log_root_dir); + + if (dp == nullptr) { + break; // ignore if we cannot access the log directory + } + + struct dirent *result = nullptr; + + int num_sess = 0, num_dates = 0; + + // There are 2 directory naming schemes: sess or --. + // For both we find the oldest and then remove the one which has more directories. + int year_min = 10000, month_min = 99, day_min = 99, sess_idx_min = 99999999, sess_idx_max = 0; + + while ((result = readdir(dp))) { + int year, month, day, sess_idx; + + if (sscanf(result->d_name, "sess%d", &sess_idx) == 1) { + ++num_sess; + + if (sess_idx > sess_idx_max) { + sess_idx_max = sess_idx; + } + + if (sess_idx < sess_idx_min) { + sess_idx_min = sess_idx; + } + + } else if (sscanf(result->d_name, "%d-%d-%d", &year, &month, &day) == 3) { + ++num_dates; + + if (year < year_min) { + year_min = year; + month_min = month; + day_min = day; + + } else if (year == year_min) { + if (month < month_min) { + month_min = month; + day_min = day; + + } else if (month == month_min) { + if (day < day_min) { + day_min = day; + } + } + } + } + } + + closedir(dp); + + sess_dir_index = sess_idx_max + 1; + + + uint64_t min_free_bytes = 300ULL * 1024ULL * 1024ULL; + uint64_t total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize; + + if (total_bytes / 10 < min_free_bytes) { // reduce the minimum if it's larger than 10% of the disk size + min_free_bytes = total_bytes / 10; + } + + if (num_sess + num_dates <= max_log_dirs_to_keep && + statfs_buf.f_bavail >= (px4_statfs_buf_f_bavail_t)(min_free_bytes / statfs_buf.f_bsize)) { + break; // enough free space and limit not reached + } + + if (num_sess == 0 && num_dates == 0) { + break; // nothing to delete + } + + char directory_to_delete[LOG_DIR_LEN]; + int n; + + if (num_sess >= num_dates) { + n = snprintf(directory_to_delete, sizeof(directory_to_delete), "%s/sess%03u", log_root_dir, sess_idx_min); + + } else { + n = snprintf(directory_to_delete, sizeof(directory_to_delete), "%s/%04u-%02u-%02u", log_root_dir, year_min, month_min, + day_min); + } + + if (n >= (int)sizeof(directory_to_delete)) { + PX4_ERR("log path too long (%i)", n); + break; + } + + PX4_INFO("removing log directory %s to get more space (left=%u MiB)", directory_to_delete, + (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize / 1024U / 1024U)); + + if (remove_directory(directory_to_delete)) { + PX4_ERR("Failed to delete directory"); + break; + } + + } while (true); + + + /* use a threshold of 50 MiB: if below, do not start logging */ + if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { + mavlink_log_critical(&mavlink_log_pub, + "[logger] Not logging; SD almost full: %u MiB", + (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize / 1024U / 1024U)); + return 1; + } + + return PX4_OK; +} + +int remove_directory(const char *dir) +{ + DIR *d = opendir(dir); + size_t dir_len = strlen(dir); + struct dirent *p; + int ret = 0; + + if (!d) { + return -1; + } + + while (!ret && (p = readdir(d))) { + int ret2 = -1; + char *buf; + size_t len; + + if (!strcmp(p->d_name, ".") || !strcmp(p->d_name, "..")) { + continue; + } + + len = dir_len + strlen(p->d_name) + 2; + buf = new char[len]; + + if (buf) { + struct stat statbuf; + + snprintf(buf, len, "%s/%s", dir, p->d_name); + + if (!stat(buf, &statbuf)) { + if (S_ISDIR(statbuf.st_mode)) { + ret2 = remove_directory(buf); + + } else { + ret2 = unlink(buf); + } + } + + delete[] buf; + } + + ret = ret2; + } + + closedir(d); + + if (!ret) { + ret = rmdir(dir); + } + + return ret; +} + +} //namespace util +} //namespace logger +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/logger/util.h b/src/prometheus_px4/src/modules/logger/util.h new file mode 100644 index 0000000..535815c --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/util.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +#ifdef __PX4_NUTTX +#define LOG_DIR_LEN 64 +#else +#define LOG_DIR_LEN 256 +#endif + +namespace px4 +{ +namespace logger +{ +namespace util +{ + +/** + * Recursively remove a directory + * @return 0 on success, <0 otherwise + */ +int remove_directory(const char *dir); + +/** + * check if a file exists + */ +bool file_exist(const char *filename); + +/** + * Check if there is enough free space left on the SD Card. + * It will remove old log files if there is not enough space, + * and if that fails return 1, and send a user message + * @param log_root_dir log root directory: it's expected to contain directories in the form of sess%i or %d-%d-%d (year, month, day) + * @param max_log_dirs_to_keep maximum log directories to keep (set to 0 for unlimited) + * @param mavlink_log_pub + * @param sess_dir_index output argument: will be set to the next free directory sess%i index. + * @return 0 on success, 1 if not enough space, <0 on error + */ +int check_free_space(const char *log_root_dir, int32_t max_log_dirs_to_keep, orb_advert_t &mavlink_log_pub, + int &sess_dir_index); + +/** + * Get the time for log file name + * @param tt returned time + * @param utc_offset_sec UTC time offset [s] + * @param boot_time use time when booted instead of current time + * @return true on success, false otherwise (eg. if no gps) + */ +bool get_log_time(struct tm *tt, int utc_offset_sec = 0, bool boot_time = false); + +} //namespace util +} //namespace logger +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/logger/watchdog.cpp b/src/prometheus_px4/src/modules/logger/watchdog.cpp new file mode 100644 index 0000000..c7328bd --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/watchdog.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "watchdog.h" + +#include + +#if defined(__PX4_NUTTX) && !defined(CONFIG_SCHED_INSTRUMENTATION) +# error watchdog support requires CONFIG_SCHED_INSTRUMENTATION +#endif + +using namespace time_literals; + +namespace px4 +{ +namespace logger +{ + +bool watchdog_update(watchdog_data_t &watchdog_data) +{ + +#ifdef __PX4_NUTTX + + if (system_load.initialized && watchdog_data.logger_main_task_index >= 0 + && watchdog_data.logger_writer_task_index >= 0) { + const hrt_abstime now = hrt_absolute_time(); + const system_load_taskinfo_s &log_writer_task = system_load.tasks[watchdog_data.logger_writer_task_index]; + + if (log_writer_task.valid) { + // Trigger the watchdog if the log writer task has been ready to run for a + // minimum duration and it has not been scheduled during that time. + // When the writer is waiting for an SD transfer, it is not in ready state, thus a long dropout + // will not trigger it. The longest period in ready state I measured was around 70ms, + // after a param change. + // We only check the log writer because it runs at lower priority than the main thread. + // No need to lock the tcb access, since we are in IRQ context + + // update the timestamp if it has been scheduled recently + if (log_writer_task.curr_start_time > watchdog_data.ready_to_run_timestamp) { + watchdog_data.ready_to_run_timestamp = log_writer_task.curr_start_time; + } + + // update the timestamp if not ready to run or if transitioned into ready to run + uint8_t current_state = log_writer_task.tcb->task_state; + + if (current_state != TSTATE_TASK_READYTORUN + || (watchdog_data.last_state != TSTATE_TASK_READYTORUN && current_state == TSTATE_TASK_READYTORUN)) { + watchdog_data.ready_to_run_timestamp = now; + } + + watchdog_data.last_state = current_state; + +#if 0 // for debugging + // test code that prints the maximum time in ready state. + // Note: we are in IRQ context, and thus are strictly speaking not allowed to use PX4_ERR - + // we do it anyway since it's only used for debugging. + static uint64_t max_time = 0; + + if (now - watchdog_data.ready_to_run_timestamp > max_time) { + max_time = now - watchdog_data.ready_to_run_timestamp; + } + + static int counter = 0; + + if (++counter > 300) { + PX4_ERR("max time in ready: %i ms", (int)max_time / 1000); + counter = 0; + max_time = 0; + } + +#endif + + if (now - watchdog_data.ready_to_run_timestamp > 1_s) { + // boost the priority to make sure the logger continues to write to the log. + // Note that we never restore the priority, to keep the logic simple and because it is + // an event that must not occur under normal circumstances (if it does, there's a bug + // somewhere) + sched_param param{}; + param.sched_priority = SCHED_PRIORITY_MAX; + + if (system_load.tasks[watchdog_data.logger_main_task_index].valid) { + sched_setparam(system_load.tasks[watchdog_data.logger_main_task_index].tcb->pid, ¶m); + } + + sched_setparam(log_writer_task.tcb->pid, ¶m); + + // make sure we won't trigger again + watchdog_data.logger_main_task_index = -1; + return true; + } + + } else { + // should never happen + watchdog_data.logger_main_task_index = -1; + } + } + +#endif /* __PX4_NUTTX */ + + return false; + +} + +void watchdog_initialize(const pid_t pid_logger_main, const pthread_t writer_thread, watchdog_data_t &watchdog_data) +{ +#ifdef __PX4_NUTTX + + // The pthread_t ID is equal to the PID on NuttX + const pthread_t pid_logger_writer = writer_thread; + + sched_lock(); // need to lock the tcb access + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid) { + if (system_load.tasks[i].tcb->pid == pid_logger_writer) { + watchdog_data.logger_writer_task_index = i; + } + + if (system_load.tasks[i].tcb->pid == pid_logger_main) { + watchdog_data.logger_main_task_index = i; + } + } + } + + sched_unlock(); + + if (watchdog_data.logger_writer_task_index == -1 || + watchdog_data.logger_main_task_index == -1) { + // If we land here it means the NuttX implementation changed + // and one of our assumptions is not valid anymore + PX4_ERR("watchdog init failed"); + } + +#endif /* __PX4_NUTTX */ +} + + +} // namespace logger +} // namespace px4 diff --git a/src/prometheus_px4/src/modules/logger/watchdog.h b/src/prometheus_px4/src/modules/logger/watchdog.h new file mode 100644 index 0000000..2c920b4 --- /dev/null +++ b/src/prometheus_px4/src/modules/logger/watchdog.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#ifdef __PX4_NUTTX +#include +#include +#endif /* __PX4_NUTTX */ + +namespace px4 +{ +namespace logger +{ + +struct watchdog_data_t { +#ifdef __PX4_NUTTX + int logger_main_task_index = -1; + int logger_writer_task_index = -1; + hrt_abstime ready_to_run_timestamp = hrt_absolute_time(); + uint8_t last_state = TSTATE_TASK_INVALID; +#endif /* __PX4_NUTTX */ +}; + + +/** + * Initialize the watchdog, fill in watchdog_data. + */ +void watchdog_initialize(const pid_t pid_logger_main, const pthread_t writer_thread, watchdog_data_t &watchdog_data); + +/** + * Update the watchdog and trigger it if necessary. It is triggered when the log writer task is in + * ready state for a certain period of time, but did not get scheduled. It means that most likely + * some other higher-prio task runs busy. + * When the watchdog triggers, it boosts the priority of the logger's main & writer tasks to maximum, so + * that they get scheduled again. + * + * Expected to be called from IRQ context. + * + * @param watchdog_data + * @return true if watchdog is triggered, false otherwise + */ +bool watchdog_update(watchdog_data_t &watchdog_data); + +} //namespace logger +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/mavlink/.gitignore b/src/prometheus_px4/src/modules/mavlink/.gitignore new file mode 100644 index 0000000..a028271 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/.gitignore @@ -0,0 +1,3 @@ +include +mavlink-* +pymavlink-* diff --git a/src/prometheus_px4/src/modules/mavlink/CMakeLists.txt b/src/prometheus_px4/src/modules/mavlink/CMakeLists.txt new file mode 100644 index 0000000..2556b17 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/CMakeLists.txt @@ -0,0 +1,82 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_git_submodule(TARGET git_mavlink_v2 PATH "${PX4_SOURCE_DIR}/mavlink/include/mavlink/v2.0") + +px4_add_module( + MODULE modules__mavlink + MAIN mavlink + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + -Wno-address-of-packed-member # TODO: fix in c_library_v2 + -Wno-enum-compare # ROTATION <-> MAV_SENSOR_ROTATION + #-DDEBUG_BUILD + INCLUDES + ${PX4_SOURCE_DIR}/mavlink/include/mavlink + SRCS + mavlink.c + mavlink_command_sender.cpp + mavlink_ftp.cpp + mavlink_log_handler.cpp + mavlink_main.cpp + mavlink_messages.cpp + mavlink_mission.cpp + mavlink_parameters.cpp + mavlink_rate_limiter.cpp + mavlink_receiver.cpp + mavlink_shell.cpp + mavlink_simple_analyzer.cpp + mavlink_stream.cpp + mavlink_timesync.cpp + mavlink_ulog.cpp + tune_publisher.cpp + MODULE_CONFIG + module.yaml + DEPENDS + airspeed + component_information_header + drivers_accelerometer + drivers_barometer + drivers_gyroscope + drivers_magnetometer + git_mavlink_v2 + conversion + git_ecl + ecl_geo + version + UNITY_BUILD + ) + +if(PX4_TESTING) + add_subdirectory(mavlink_tests) +endif() diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink.c b/src/prometheus_px4/src/modules/mavlink/mavlink.c new file mode 100644 index 0000000..444c8f6 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink.c + * Define MAVLink specific parameters + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include "mavlink_bridge_header.h" +#include + +mavlink_system_t mavlink_system = { + 1, + 1 +}; // System ID, 1-255, Component/Subsystem ID, 1-255 diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_bridge_header.h b/src/prometheus_px4/src/modules/mavlink/mavlink_bridge_header.h new file mode 100644 index 0000000..e4811f5 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_bridge_header.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink_bridge_header + * MAVLink bridge header for UART access. + * + * @author Lorenz Meier + */ + +/* MAVLink adapter header */ +#ifndef MAVLINK_BRIDGE_HEADER_H +#define MAVLINK_BRIDGE_HEADER_H + +#define MAVLINK_NO_CONVERSION_HELPERS + +#define MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/* use efficient approach, see mavlink_helpers.h */ +#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes + +#define MAVLINK_START_UART_SEND mavlink_start_uart_send +#define MAVLINK_END_UART_SEND mavlink_end_uart_send + +#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer +#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status + +#if !defined(CONSTRAINED_MEMORY) +# define MAVLINK_COMM_NUM_BUFFERS 6 +# define MAVLINK_COMM_4 static_cast(4) +# define MAVLINK_COMM_5 static_cast(5) +#endif + +#include +#include + +__BEGIN_DECLS + +/* Struct that stores the communication settings of this system. + you can also define / alter these settings elsewhere, as long + as they're included BEFORE mavlink.h. + So you can set the + + mavlink_system.sysid = 100; // System ID, 1-255 + mavlink_system.compid = 50; // Component/Subsystem ID, 1-255 + + Lines also in your main.c, e.g. by reading these parameter from EEPROM. + */ +extern mavlink_system_t mavlink_system; + +/** + * @brief Send multiple chars (uint8_t) over a comm channel + * + * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 + * @param ch Character to send + */ +void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); + +void mavlink_start_uart_send(mavlink_channel_t chan, int length); +void mavlink_end_uart_send(mavlink_channel_t chan, int length); + +extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); +extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); + +#include + +__END_DECLS + +#endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_command_sender.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_command_sender.cpp new file mode 100644 index 0000000..a25b6aa --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_command_sender.cpp @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mavlink_command_sender.cpp + * Mavlink commands sender with support for retransmission. + * + * @author Julian Oes + */ + +#include "mavlink_command_sender.h" +#include + +#define CMD_DEBUG(FMT, ...) PX4_LOG_NAMED_COND("cmd sender", _debug_enabled, FMT, ##__VA_ARGS__) + +MavlinkCommandSender *MavlinkCommandSender::_instance = nullptr; +px4_sem_t MavlinkCommandSender::_lock; + +void MavlinkCommandSender::initialize() +{ + px4_sem_init(&_lock, 1, 1); + + if (_instance == nullptr) { + _instance = new MavlinkCommandSender(); + } +} + +MavlinkCommandSender &MavlinkCommandSender::instance() +{ + return *_instance; +} + +MavlinkCommandSender::~MavlinkCommandSender() +{ + px4_sem_destroy(&_lock); +} + +int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel) +{ + // commands > uint16 are PX4 internal only + if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { + return 0; + } + + lock(); + CMD_DEBUG("new command: %" PRIu32 " (channel: %d)", command.command, channel); + + mavlink_command_long_t msg = {}; + msg.target_system = command.target_system; + msg.target_component = command.target_component; + msg.command = command.command; + msg.confirmation = command.confirmation; + msg.param1 = command.param1; + msg.param2 = command.param2; + msg.param3 = command.param3; + msg.param4 = command.param4; + msg.param5 = command.param5; + msg.param6 = command.param6; + msg.param7 = command.param7; + mavlink_msg_command_long_send_struct(channel, &msg); + + bool already_existing = false; + _commands.reset_to_start(); + + while (command_item_s *item = _commands.get_next()) { + if (item->timestamp_us == command.timestamp) { + + // We should activate the channel by setting num_sent_per_channel from -1 to 0. + item->num_sent_per_channel[channel] = 0; + already_existing = true; + break; + } + } + + if (!already_existing) { + + command_item_s new_item; + new_item.command = msg; + new_item.timestamp_us = command.timestamp; + new_item.num_sent_per_channel[channel] = 0; + new_item.last_time_sent_us = hrt_absolute_time(); + _commands.put(new_item); + } + + unlock(); + return 0; +} + +void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_t &ack, + uint8_t from_sysid, uint8_t from_compid, uint8_t channel) +{ + CMD_DEBUG("handling result %" PRIu8 " for command %" PRIu16 " (from %" PRIu8 ":%" PRIu8 ")", + ack.result, ack.command, from_sysid, from_compid); + lock(); + + _commands.reset_to_start(); + + while (command_item_s *item = _commands.get_next()) { + // Check if the incoming ack matches any of the commands that we have sent. + if (item->command.command == ack.command && + (item->command.target_system == 0 || from_sysid == item->command.target_system) && + (item->command.target_component == 0 || from_compid == item->command.target_component) && + item->num_sent_per_channel[channel] != -1) { + item->num_sent_per_channel[channel] = -2; // mark this as acknowledged + break; + } + } + + unlock(); +} + +void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) +{ + lock(); + + _commands.reset_to_start(); + + while (command_item_s *item = _commands.get_next()) { + if (hrt_elapsed_time(&item->last_time_sent_us) <= TIMEOUT_US) { + // We keep waiting for the timeout. + continue; + } + + // Loop through num_sent_per_channel and check if any channel has receives an ack for this command + // (indicated by the value -2). We avoid removing the command at the time of receiving the ack + // as some channels might be lagging behind and will end up putting the same command into the buffer. + bool dropped_command = false; + + for (unsigned i = 0; i < MAVLINK_COMM_NUM_BUFFERS; ++i) { + if (item->num_sent_per_channel[i] == -2) { + _commands.drop_current(); + dropped_command = true; + break; + } + } + + if (dropped_command) { + continue; + } + + // The goal of this is to retry from all channels. Therefore, we keep + // track of the retry count for each channel. + // + // When the first channel does a retry, the timeout is reset. + // (e.g. all channel have done 2 retries, then channel 0 is called + // and does retry number 3, and also resets the timeout timestamp). + + // First, we need to determine what the current max and min retry level + // are because we can only level up, if all have caught up. + // If num_sent_per_channel is at -1, the channel is inactive. + int8_t max_sent = 0; + int8_t min_sent = INT8_MAX; + + for (unsigned i = 0; i < MAVLINK_COMM_NUM_BUFFERS; ++i) { + if (item->num_sent_per_channel[i] > max_sent) { + max_sent = item->num_sent_per_channel[i]; + } + + if ((item->num_sent_per_channel[i] != -1) && + (item->num_sent_per_channel[i] < min_sent)) { + min_sent = item->num_sent_per_channel[i]; + } + } + + if (item->num_sent_per_channel[channel] < max_sent && item->num_sent_per_channel[channel] != -1) { + // We are behind and need to do a retransmission. + item->command.confirmation = ++item->num_sent_per_channel[channel]; + mavlink_msg_command_long_send_struct(channel, &item->command); + + CMD_DEBUG("command %" PRIu16 " sent (not first, retries: %" PRIu8 "/%" PRIi8 ", channel: %d)", + item->command.command, + item->num_sent_per_channel[channel], + max_sent, + channel); + + } else if (item->num_sent_per_channel[channel] == max_sent && + min_sent == max_sent) { + + // If the next retry would be above the needed retries anyway, we can + // drop the item, and continue with other items. + if (item->num_sent_per_channel[channel] + 1 > RETRIES) { + CMD_DEBUG("command %" PRIu16 " dropped", item->command.command); + _commands.drop_current(); + continue; + } + + // We are the first of a new retransmission series. + item->command.confirmation = ++item->num_sent_per_channel[channel]; + mavlink_msg_command_long_send_struct(channel, &item->command); + // Therefore, we are the ones setting the timestamp of this retry round. + item->last_time_sent_us = hrt_absolute_time(); + + CMD_DEBUG("command %" PRIu16 " sent (first, retries: %" PRId8 "/%" PRId8 ", channel: %d)", + item->command.command, + item->num_sent_per_channel[channel], + max_sent, + channel); + + } else { + // We are already ahead, so this should not happen. + // If it ever does, just ignore it. It will timeout eventually. + continue; + } + } + + unlock(); +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_command_sender.h b/src/prometheus_px4/src/modules/mavlink/mavlink_command_sender.h new file mode 100644 index 0000000..9d5eb82 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_command_sender.h @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 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 mavlink_command_sender.h + * Mavlink commands sender with support for retransmission. + * + * @author Julian Oes + */ + +#pragma once + +#include +#include +#include + +#include +#include + +#include "timestamped_list.h" +#include "mavlink_bridge_header.h" + +/** + * @class MavlinkCommandSender + */ +class MavlinkCommandSender +{ +public: + /** + * initialize: call this once on startup (this function is not thread-safe!) + */ + static void initialize(); + + static MavlinkCommandSender &instance(); + + /** + * Send a command on a channel and keep it in a queue for retransmission. + * thread-safe + * @return 0 on success, <0 otherwise + */ + int handle_vehicle_command(const struct vehicle_command_s &command, mavlink_channel_t channel); + + /** + * Check timeouts to verify if an commands need retransmission. + * thread-safe + */ + void check_timeout(mavlink_channel_t channel); + + /** + * Handle mavlink command_ack. + * thread-safe + */ + void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid, + uint8_t channel); + +private: + MavlinkCommandSender() = default; + + ~MavlinkCommandSender(); + + static void lock() + { + do {} while (px4_sem_wait(&_lock) != 0); + } + + static void unlock() + { + px4_sem_post(&_lock); + } + + static MavlinkCommandSender *_instance; + static px4_sem_t _lock; + + struct command_item_s { + mavlink_command_long_t command = {}; + hrt_abstime timestamp_us = 0; + hrt_abstime last_time_sent_us = 0; + // -1: channel did not request this command to be sent, -2: channel got an ack for this command +#if MAVLINK_COMM_NUM_BUFFERS == 4 + int8_t num_sent_per_channel[MAVLINK_COMM_NUM_BUFFERS] {-1, -1, -1, -1}; +#elif MAVLINK_COMM_NUM_BUFFERS == 6 + int8_t num_sent_per_channel[MAVLINK_COMM_NUM_BUFFERS] {-1, -1, -1, -1, -1, -1}; +#else +# error Unknown number of MAVLINK_COMM_NUM_BUFFERS +#endif + }; + + TimestampedList _commands{3}; + + bool _debug_enabled = false; + static constexpr uint8_t RETRIES = 3; + static constexpr uint64_t TIMEOUT_US = 500000; + + /* do not allow copying or assigning this class */ + MavlinkCommandSender(const MavlinkCommandSender &) = delete; + MavlinkCommandSender operator=(const MavlinkCommandSender &) = delete; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_ftp.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_ftp.cpp new file mode 100644 index 0000000..10062f7 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_ftp.cpp @@ -0,0 +1,1106 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 mavlink_ftp.cpp +/// @author px4dev, Don Gagne + +#include +#include +#include +#include +#include +#include +#include + +#include "mavlink_ftp.h" +#include "mavlink_tests/mavlink_ftp_test.h" + +#ifndef MAVLINK_FTP_UNIT_TEST +#include "mavlink_main.h" +#else +#include +#endif + +using namespace time_literals; + +constexpr const char MavlinkFTP::_root_dir[]; + +MavlinkFTP::MavlinkFTP(Mavlink *mavlink) : + _mavlink(mavlink) +{ + // initialize session + _session_info.fd = -1; +} + +MavlinkFTP::~MavlinkFTP() +{ + delete[] _work_buffer1; + delete[] _work_buffer2; +} + +unsigned +MavlinkFTP::get_size() +{ + if (_session_info.stream_download) { + return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + +#ifdef MAVLINK_FTP_UNIT_TEST +void +MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data) +{ + _utRcvMsgFunc = rcvMsgFunc; + _worker_data = worker_data; +} +#endif + +uint8_t +MavlinkFTP::_getServerSystemId() +{ +#ifdef MAVLINK_FTP_UNIT_TEST + // We use fake ids when unit testing + return MavlinkFtpTest::serverSystemId; +#else + // Not unit testing, use the real thing + return _mavlink->get_system_id(); +#endif +} + +uint8_t +MavlinkFTP::_getServerComponentId() +{ +#ifdef MAVLINK_FTP_UNIT_TEST + // We use fake ids when unit testing + return MavlinkFtpTest::serverComponentId; +#else + // Not unit testing, use the real thing + return _mavlink->get_component_id(); +#endif +} + +uint8_t +MavlinkFTP::_getServerChannel() +{ +#ifdef MAVLINK_FTP_UNIT_TEST + // We use fake ids when unit testing + return MavlinkFtpTest::serverChannel; +#else + // Not unit testing, use the real thing + return _mavlink->get_channel(); +#endif +} + +void +MavlinkFTP::handle_message(const mavlink_message_t *msg) +{ + //warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2); + + if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + mavlink_file_transfer_protocol_t ftp_request; + mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request); + +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("FTP: received ftp protocol message target_system: %d target_component: %d", + ftp_request.target_system, ftp_request.target_component); +#endif + + if ((ftp_request.target_system == _getServerSystemId() || ftp_request.target_system == 0) && + (ftp_request.target_component == _getServerComponentId() || ftp_request.target_component == 0)) { + _process_request(&ftp_request, msg->sysid, msg->compid); + } + } +} + +/// @brief Processes an FTP message +void +MavlinkFTP::_process_request( + mavlink_file_transfer_protocol_t *ftp_req, + uint8_t target_system_id, + uint8_t target_comp_id) +{ + bool stream_send = false; + PayloadHeader *payload = reinterpret_cast(&ftp_req->payload[0]); + + ErrorCode errorCode = kErrNone; + + if (!_ensure_buffers_exist()) { + PX4_ERR("Failed to allocate buffers"); + errorCode = kErrFailErrno; + errno = ENOMEM; + goto out; + } + + // basic sanity checks; must validate length before use + if (payload->size > kMaxDataLength) { + errorCode = kErrInvalidDataSize; + goto out; + } + + // check the sequence number: if this is a resent request, resend the last response + if (_last_reply_valid) { + mavlink_file_transfer_protocol_t *last_reply = reinterpret_cast(_last_reply); + PayloadHeader *last_payload = reinterpret_cast(&last_reply->payload[0]); + + if (payload->seq_number + 1 == last_payload->seq_number) { + // this is the same request as the one we replied to last. It means the (n)ack got lost, and the GCS + // resent the request +#ifdef MAVLINK_FTP_UNIT_TEST + _utRcvMsgFunc(last_reply, _worker_data); +#else + mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), last_reply); +#endif + return; + } + } + + + +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("ftp: channel %" PRIu8 " opc %" PRIu8 " size %" PRIu8 " offset %" PRIu32, _getServerChannel(), payload->opcode, + payload->size, + payload->offset); +#endif + + switch (payload->opcode) { + case kCmdNone: + break; + + case kCmdTerminateSession: + errorCode = _workTerminate(payload); + break; + + case kCmdResetSessions: + errorCode = _workReset(payload); + break; + + case kCmdListDirectory: + errorCode = _workList(payload); + break; + + case kCmdOpenFileRO: + errorCode = _workOpen(payload, O_RDONLY); + break; + + case kCmdCreateFile: + errorCode = _workOpen(payload, O_CREAT | O_TRUNC | O_WRONLY); + break; + + case kCmdOpenFileWO: + errorCode = _workOpen(payload, O_CREAT | O_WRONLY); + break; + + case kCmdReadFile: + errorCode = _workRead(payload); + break; + + case kCmdBurstReadFile: + errorCode = _workBurst(payload, target_system_id, target_comp_id); + stream_send = true; + break; + + case kCmdWriteFile: + errorCode = _workWrite(payload); + break; + + case kCmdRemoveFile: + errorCode = _workRemoveFile(payload); + break; + + case kCmdRename: + errorCode = _workRename(payload); + break; + + case kCmdTruncateFile: + errorCode = _workTruncateFile(payload); + break; + + case kCmdCreateDirectory: + errorCode = _workCreateDirectory(payload); + break; + + case kCmdRemoveDirectory: + errorCode = _workRemoveDirectory(payload); + break; + + case kCmdCalcFileCRC32: + errorCode = _workCalcFileCRC32(payload); + break; + + default: + errorCode = kErrUnknownCommand; + break; + } + +out: + payload->seq_number++; + + // handle success vs. error + if (errorCode == kErrNone) { + payload->req_opcode = payload->opcode; + payload->opcode = kRspAck; + + } else { + int r_errno = errno; + payload->req_opcode = payload->opcode; + payload->opcode = kRspNak; + payload->size = 1; + + if (r_errno == EEXIST) { + errorCode = kErrFailFileExists; + + } else if (r_errno == ENOENT && errorCode == kErrFailErrno) { + errorCode = kErrFileNotFound; + } + + payload->data[0] = errorCode; + + if (errorCode == kErrFailErrno) { + payload->size = 2; + payload->data[1] = r_errno; + } + } + + _last_reply_valid = false; + + // Stream download replies are sent through mavlink stream mechanism. Unless we need to Nack. + if (!stream_send || errorCode != kErrNone) { + // respond to the request + ftp_req->target_system = target_system_id; + ftp_req->target_network = 0; + ftp_req->target_component = target_comp_id; + _reply(ftp_req); + } +} + +bool MavlinkFTP::_ensure_buffers_exist() +{ + _last_work_buffer_access = hrt_absolute_time(); + + if (!_work_buffer1) { + _work_buffer1 = new char[_work_buffer1_len]; + } + + if (!_work_buffer2) { + _work_buffer2 = new char[_work_buffer2_len]; + } + + return _work_buffer1 && _work_buffer2; +} + +/// @brief Sends the specified FTP response message out through mavlink +void +MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req) +{ + PayloadHeader *payload = reinterpret_cast(&ftp_req->payload[0]); + + // keep a copy of the last sent response ((n)ack), so that if it gets lost and the GCS resends the request, + // we can simply resend the response. + // we only keep small responses to reduce RAM usage and avoid large memcpy's. The larger responses are all data + // retrievals without side-effects, meaning it's ok to reexecute them if a response gets lost + if (payload->size <= sizeof(uint32_t)) { + _last_reply_valid = true; + memcpy(_last_reply, ftp_req, sizeof(_last_reply)); + } + +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("FTP: %s seq_number: %" PRIu16, payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number); +#endif + +#ifdef MAVLINK_FTP_UNIT_TEST + // Unit test hook is set, call that instead + _utRcvMsgFunc(ftp_req, _worker_data); +#else + mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req); +#endif + +} + +/// @brief Responds to a List command +MavlinkFTP::ErrorCode +MavlinkFTP::_workList(PayloadHeader *payload) +{ + strncpy(_work_buffer1, _root_dir, _work_buffer1_len); + strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); + // ensure termination + _work_buffer1[_work_buffer1_len - 1] = '\0'; + + ErrorCode errorCode = kErrNone; + unsigned offset = 0; + + DIR *dp = opendir(_work_buffer1); + + if (dp == nullptr) { + PX4_WARN("File open failed %s", _work_buffer1); + return kErrFileNotFound; + } + +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("FTP: list %s offset %" PRIu32, _work_buffer1, payload->offset); +#endif + + struct dirent *result = nullptr; + + // move to the requested offset + int requested_offset = payload->offset; + + while (requested_offset-- > 0 && readdir(dp)) {} + + for (;;) { + errno = 0; + result = readdir(dp); + + // read the directory entry + if (result == nullptr) { + if (errno) { + PX4_WARN("readdir failed"); + payload->data[offset++] = kDirentSkip; + *((char *)&payload->data[offset]) = '\0'; + offset++; + payload->size = offset; + closedir(dp); + + return errorCode; + } + + // no more entries? + if (payload->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that + break; + } + + uint32_t fileSize = 0; + char direntType; + + // Determine the directory entry type + switch (result->d_type) { +#ifdef __PX4_NUTTX + + case DTYPE_FILE: { +#else + + case DT_REG: { +#endif + // For files we get the file size as well + direntType = kDirentFile; + int ret = snprintf(_work_buffer2, _work_buffer2_len, "%s/%s", _work_buffer1, result->d_name); + bool buf_is_ok = ((ret > 0) && (ret < _work_buffer2_len)); + + if (buf_is_ok) { + struct stat st; + + if (stat(_work_buffer2, &st) == 0) { + fileSize = st.st_size; + } + } + + break; + } + +#ifdef __PX4_NUTTX + + case DTYPE_DIRECTORY: +#else + case DT_DIR: +#endif + if (strcmp(result->d_name, ".") == 0 || strcmp(result->d_name, "..") == 0) { + // Don't bother sending these back + direntType = kDirentSkip; + + } else { + direntType = kDirentDir; + } + + break; + + default: + // We only send back file and diretory entries, skip everything else + direntType = kDirentSkip; + } + + if (direntType == kDirentSkip) { + // Skip send only dirent identifier + _work_buffer2[0] = '\0'; + + } else if (direntType == kDirentFile) { + // Files send filename and file length + int ret = snprintf(_work_buffer2, _work_buffer2_len, "%s\t%" PRIu32, result->d_name, fileSize); + bool buf_is_ok = ((ret > 0) && (ret < _work_buffer2_len)); + + if (!buf_is_ok) { + _work_buffer2[_work_buffer2_len - 1] = '\0'; + } + + } else { + // Everything else just sends name + strncpy(_work_buffer2, result->d_name, _work_buffer2_len); + _work_buffer2[_work_buffer2_len - 1] = '\0'; + } + + size_t nameLen = strlen(_work_buffer2); + + // Do we have room for the name, the one char directory identifier and the null terminator? + if ((offset + nameLen + 2) > kMaxDataLength) { + break; + } + + // Move the data into the buffer + payload->data[offset++] = direntType; + strcpy((char *)&payload->data[offset], _work_buffer2); +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("FTP: list %s %s", _work_buffer1, (char *)&payload->data[offset - 1]); +#endif + offset += nameLen + 1; + } + + closedir(dp); + payload->size = offset; + + return errorCode; +} + +/// @brief Responds to an Open command +MavlinkFTP::ErrorCode +MavlinkFTP::_workOpen(PayloadHeader *payload, int oflag) +{ + if (_session_info.fd >= 0) { + PX4_ERR("FTP: Open failed - out of sessions\n"); + return kErrNoSessionsAvailable; + } + + strncpy(_work_buffer1, _root_dir, _work_buffer1_len); + strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); + +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("FTP: open '%s'", _work_buffer1); +#endif + + uint32_t fileSize = 0; + struct stat st; + + if (stat(_work_buffer1, &st) != 0) { + // fail only if requested open for read + if (oflag & O_RDONLY) { + return kErrFailErrno; + + } else { + st.st_size = 0; + } + } + + fileSize = st.st_size; + + // Set mode to 666 incase oflag has O_CREAT + int fd = ::open(_work_buffer1, oflag, PX4_O_MODE_666); + + if (fd < 0) { + return kErrFailErrno; + } + + _session_info.fd = fd; + _session_info.file_size = fileSize; + _session_info.stream_download = false; + + payload->session = 0; + payload->size = sizeof(uint32_t); + std::memcpy(payload->data, &fileSize, payload->size); + + return kErrNone; +} + +/// @brief Responds to a Read command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRead(PayloadHeader *payload) +{ + if (payload->session != 0 || _session_info.fd < 0) { + return kErrInvalidSession; + } + +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("FTP: read offset:%" PRIu32, payload->offset); +#endif + + // We have to test seek past EOF ourselves, lseek will allow seek past EOF + if (payload->offset >= _session_info.file_size) { + PX4_ERR("request past EOF"); + return kErrEOF; + } + + if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { + PX4_ERR("seek fail"); + return kErrFailErrno; + } + + int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength); + + if (bytes_read < 0) { + // Negative return indicates error other than eof + PX4_ERR("read fail %d", bytes_read); + return kErrFailErrno; + } + + payload->size = bytes_read; + + return kErrNone; +} + +/// @brief Responds to a Stream command +MavlinkFTP::ErrorCode +MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id, uint8_t target_component_id) +{ + if (payload->session != 0 && _session_info.fd < 0) { + return kErrInvalidSession; + } + +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("FTP: burst offset:%" PRIu32, payload->offset); +#endif + // Setup for streaming sends + _session_info.stream_download = true; + _session_info.stream_offset = payload->offset; + _session_info.stream_chunk_transmitted = 0; + _session_info.stream_seq_number = payload->seq_number + 1; + _session_info.stream_target_system_id = target_system_id; + _session_info.stream_target_component_id = target_component_id; + + return kErrNone; +} + +/// @brief Responds to a Write command +MavlinkFTP::ErrorCode +MavlinkFTP::_workWrite(PayloadHeader *payload) +{ + if (payload->session != 0 && _session_info.fd < 0) { + return kErrInvalidSession; + } + + if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + PX4_ERR("seek fail"); + return kErrFailErrno; + } + + int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size); + + if (bytes_written < 0) { + // Negative return indicates error other than eof + PX4_ERR("write fail %d", bytes_written); + return kErrFailErrno; + } + + payload->size = sizeof(uint32_t); + std::memcpy(payload->data, &bytes_written, payload->size); + + return kErrNone; +} + +/// @brief Responds to a RemoveFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveFile(PayloadHeader *payload) +{ + strncpy(_work_buffer1, _root_dir, _work_buffer1_len); + strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); + // ensure termination + _work_buffer1[_work_buffer1_len - 1] = '\0'; + + if (unlink(_work_buffer1) == 0) { + payload->size = 0; + return kErrNone; + + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a TruncateFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTruncateFile(PayloadHeader *payload) +{ + strncpy(_work_buffer1, _root_dir, _work_buffer1_len); + strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); + // ensure termination + _work_buffer1[_work_buffer1_len - 1] = '\0'; + payload->size = 0; + +#ifdef __PX4_NUTTX + + // emulate truncate(_work_buffer1, payload->offset) by + // copying to temp and overwrite with O_TRUNC flag (NuttX does not support truncate()). + const char temp_file[] = PX4_STORAGEDIR"/.trunc.tmp"; + + struct stat st; + + if (stat(_work_buffer1, &st) != 0) { + return kErrFailErrno; + } + + if (!S_ISREG(st.st_mode)) { + errno = EISDIR; + return kErrFailErrno; + } + + // check perms allow us to write (not romfs) + if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) { + errno = EROFS; + return kErrFailErrno; + } + + if (payload->offset == (unsigned)st.st_size) { + // nothing to do + return kErrNone; + + } else if (payload->offset == 0) { + // 1: truncate all data + int fd = ::open(_work_buffer1, O_TRUNC | O_WRONLY); + + if (fd < 0) { + return kErrFailErrno; + } + + ::close(fd); + return kErrNone; + + } else if (payload->offset > (unsigned)st.st_size) { + // 2: extend file + int fd = ::open(_work_buffer1, O_WRONLY); + + if (fd < 0) { + return kErrFailErrno; + } + + if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) { + ::close(fd); + return kErrFailErrno; + } + + bool ok = 1 == ::write(fd, "", 1); + ::close(fd); + + return (ok) ? kErrNone : kErrFailErrno; + + } else { + // 3: truncate + if (_copy_file(_work_buffer1, temp_file, payload->offset) != 0) { + return kErrFailErrno; + } + + if (_copy_file(temp_file, _work_buffer1, payload->offset) != 0) { + return kErrFailErrno; + } + + if (::unlink(temp_file) != 0) { + return kErrFailErrno; + } + + return kErrNone; + } + +#else + int ret = truncate(_work_buffer1, payload->offset); + + if (ret == 0) { + return kErrNone; + } + + return kErrFailErrno; +#endif /* __PX4_NUTTX */ +} + +/// @brief Responds to a Terminate command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTerminate(PayloadHeader *payload) +{ + if (payload->session != 0 || _session_info.fd < 0) { + return kErrInvalidSession; + } + + ::close(_session_info.fd); + _session_info.fd = -1; + _session_info.stream_download = false; + + payload->size = 0; + + return kErrNone; +} + +/// @brief Responds to a Reset command +MavlinkFTP::ErrorCode +MavlinkFTP::_workReset(PayloadHeader *payload) +{ + if (_session_info.fd != -1) { + ::close(_session_info.fd); + _session_info.fd = -1; + _session_info.stream_download = false; + } + + payload->size = 0; + + return kErrNone; +} + +/// @brief Responds to a Rename command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRename(PayloadHeader *payload) +{ + char *ptr = _data_as_cstring(payload); + size_t oldpath_sz = strlen(ptr); + + if (oldpath_sz == payload->size) { + // no newpath + errno = EINVAL; + return kErrFailErrno; + } + + strncpy(_work_buffer1, _root_dir, _work_buffer1_len); + strncpy(_work_buffer1 + _root_dir_len, ptr, _work_buffer1_len - _root_dir_len); + _work_buffer1[_work_buffer1_len - 1] = '\0'; // ensure termination + + strncpy(_work_buffer2, _root_dir, _work_buffer2_len); + strncpy(_work_buffer2 + _root_dir_len, ptr + oldpath_sz + 1, _work_buffer2_len - _root_dir_len); + _work_buffer2[_work_buffer2_len - 1] = '\0'; // ensure termination + + if (rename(_work_buffer1, _work_buffer2) == 0) { + payload->size = 0; + return kErrNone; + + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a RemoveDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveDirectory(PayloadHeader *payload) +{ + strncpy(_work_buffer1, _root_dir, _work_buffer1_len); + strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); + // ensure termination + _work_buffer1[_work_buffer1_len - 1] = '\0'; + + if (rmdir(_work_buffer1) == 0) { + payload->size = 0; + return kErrNone; + + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CreateDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCreateDirectory(PayloadHeader *payload) +{ + strncpy(_work_buffer1, _root_dir, _work_buffer1_len); + strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); + // ensure termination + _work_buffer1[_work_buffer1_len - 1] = '\0'; + + if (mkdir(_work_buffer1, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { + payload->size = 0; + return kErrNone; + + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CalcFileCRC32 command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCalcFileCRC32(PayloadHeader *payload) +{ + uint32_t checksum = 0; + ssize_t bytes_read; + strncpy(_work_buffer2, _root_dir, _work_buffer2_len); + strncpy(_work_buffer2 + _root_dir_len, _data_as_cstring(payload), _work_buffer2_len - _root_dir_len); + // ensure termination + _work_buffer2[_work_buffer2_len - 1] = '\0'; + + int fd = ::open(_work_buffer2, O_RDONLY); + + if (fd < 0) { + return kErrFailErrno; + } + + do { + bytes_read = ::read(fd, _work_buffer2, _work_buffer2_len); + + if (bytes_read < 0) { + int r_errno = errno; + ::close(fd); + errno = r_errno; + return kErrFailErrno; + } + + checksum = crc32part((uint8_t *)_work_buffer2, bytes_read, checksum); + } while (bytes_read == _work_buffer2_len); + + ::close(fd); + + payload->size = sizeof(uint32_t); + std::memcpy(payload->data, &checksum, payload->size); + return kErrNone; +} + +/// @brief Guarantees that the payload data is null terminated. +/// @return Returns a pointer to the payload data as a char * +char * +MavlinkFTP::_data_as_cstring(PayloadHeader *payload) +{ + // guarantee nul termination + if (payload->size < kMaxDataLength) { + payload->data[payload->size] = '\0'; + + } else { + payload->data[kMaxDataLength - 1] = '\0'; + } + + // and return data + return (char *) & (payload->data[0]); +} + +/// @brief Copy file (with limited space) +int +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length) +{ + int src_fd = -1, dst_fd = -1; + int op_errno = 0; + + src_fd = ::open(src_path, O_RDONLY); + + if (src_fd < 0) { + return -1; + } + + dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY +// POSIX requires the permissions to be supplied if O_CREAT passed +#ifdef __PX4_POSIX + , 0666 +#endif + ); + + if (dst_fd < 0) { + op_errno = errno; + ::close(src_fd); + errno = op_errno; + return -1; + } + + while (length > 0) { + ssize_t bytes_read, bytes_written; + size_t blen = (length > _work_buffer2_len) ? _work_buffer2_len : length; + + bytes_read = ::read(src_fd, _work_buffer2, blen); + + if (bytes_read == 0) { + // EOF + break; + + } else if (bytes_read < 0) { + PX4_ERR("cp: read"); + op_errno = errno; + break; + } + + bytes_written = ::write(dst_fd, _work_buffer2, bytes_read); + + if (bytes_written != bytes_read) { + PX4_ERR("cp: short write"); + op_errno = errno; + break; + } + + length -= bytes_written; + } + + ::close(src_fd); + ::close(dst_fd); + + errno = op_errno; + return (length > 0) ? -1 : 0; +} + +void MavlinkFTP::send() +{ + + if (_work_buffer1 || _work_buffer2) { + // free the work buffers if they are not used for a while + if (hrt_elapsed_time(&_last_work_buffer_access) > 2_s) { + if (_work_buffer1) { + delete[] _work_buffer1; + _work_buffer1 = nullptr; + } + + if (_work_buffer2) { + delete[] _work_buffer2; + _work_buffer2 = nullptr; + } + } + + } else if (_session_info.fd != -1) { + // close session without activity + if (hrt_elapsed_time(&_last_work_buffer_access) > 10_s) { + ::close(_session_info.fd); + _session_info.fd = -1; + _session_info.stream_download = false; + _last_reply_valid = false; + PX4_WARN("Session was closed without activity"); + } + } + + // Anything to stream? + if (!_session_info.stream_download) { + return; + } + +#ifndef MAVLINK_FTP_UNIT_TEST + // Skip send if not enough room + unsigned max_bytes_to_send = _mavlink->get_free_tx_buf(); +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("MavlinkFTP::send max_bytes_to_send(%u) get_free_tx_buf(%u)", max_bytes_to_send, _mavlink->get_free_tx_buf()); +#endif + + if (max_bytes_to_send < get_size()) { + return; + } + +#endif + + // Send stream packets until buffer is full + + bool more_data; + + do { + more_data = false; + + ErrorCode error_code = kErrNone; + + mavlink_file_transfer_protocol_t ftp_msg; + PayloadHeader *payload = reinterpret_cast(&ftp_msg.payload[0]); + + payload->seq_number = _session_info.stream_seq_number; + payload->session = 0; + payload->opcode = kRspAck; + payload->req_opcode = kCmdBurstReadFile; + payload->offset = _session_info.stream_offset; + _session_info.stream_seq_number++; + +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("stream send: offset %" PRIu32, _session_info.stream_offset); +#endif + + // We have to test seek past EOF ourselves, lseek will allow seek past EOF + if (_session_info.stream_offset >= _session_info.file_size) { + error_code = kErrEOF; +#ifdef MAVLINK_FTP_DEBUG + PX4_INFO("stream download: sending Nak EOF"); +#endif + } + + if (error_code == kErrNone) { + if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) { + error_code = kErrFailErrno; +#ifdef MAVLINK_FTP_DEBUG + PX4_WARN("stream download: seek fail"); +#endif + } + } + + if (error_code == kErrNone) { + int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength); + + if (bytes_read < 0) { + // Negative return indicates error other than eof + error_code = kErrFailErrno; +#ifdef MAVLINK_FTP_DEBUG + PX4_WARN("stream download: read fail"); +#endif + + } else { + payload->size = bytes_read; + _session_info.stream_offset += bytes_read; + _session_info.stream_chunk_transmitted += bytes_read; + } + } + + if (error_code != kErrNone) { + payload->opcode = kRspNak; + payload->size = 1; + uint8_t *pData = &payload->data[0]; + *pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error + + if (error_code == kErrFailErrno) { + int r_errno = errno; + payload->size = 2; + payload->data[1] = r_errno; + } + + _session_info.stream_download = false; + + } else { +#ifndef MAVLINK_FTP_UNIT_TEST + + if (max_bytes_to_send < (get_size() * 2)) { + more_data = false; + + /* perform transfers in 35K chunks - this is determined empirical */ + if (_session_info.stream_chunk_transmitted > 35000) { + payload->burst_complete = true; + _session_info.stream_download = false; + _session_info.stream_chunk_transmitted = 0; + } + + } else { +#endif + more_data = true; + payload->burst_complete = false; +#ifndef MAVLINK_FTP_UNIT_TEST + max_bytes_to_send -= get_size(); + } + +#endif + } + + ftp_msg.target_system = _session_info.stream_target_system_id; + ftp_msg.target_network = 0; + ftp_msg.target_component = _session_info.stream_target_component_id; + _reply(&ftp_msg); + } while (more_data); +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_ftp.h b/src/prometheus_px4/src/modules/mavlink/mavlink_ftp.h new file mode 100644 index 0000000..a796ce4 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_ftp.h @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 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. + * + ****************************************************************************/ + +#pragma once + +/// @file mavlink_ftp.h +/// @author px4dev, Don Gagne + +#include +#include + +#include +#include +#include + +#ifndef MAVLINK_FTP_UNIT_TEST +#include "mavlink_bridge_header.h" +#else +#include +#endif + +class MavlinkFtpTest; +class Mavlink; + +/// MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message. +class MavlinkFTP +{ +public: + MavlinkFTP(Mavlink *mavlink); + ~MavlinkFTP(); + + /** + * Handle sending of messages. Call this regularly at a fixed frequency. + * @param t current time + */ + void send(); + + /// Handle possible FTP message + void handle_message(const mavlink_message_t *msg); + + typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data); + + /// @brief Sets up the server to run in unit test mode. + /// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages. + /// @param worker_data Data to pass to worker + void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data); + + /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. + /// This needs to be packed, because it's typecasted from mavlink_file_transfer_protocol_t.payload, which starts + /// at a 3 byte offset, causing an unaligned access to seq_number and offset + struct __attribute__((__packed__)) PayloadHeader { + uint16_t seq_number; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message + uint8_t burst_complete; ///< Only used if req_opcode=kCmdBurstReadFile - 1: set of burst packets complete, 0: More burst packets coming. + uint8_t padding; ///< 32 bit aligment padding + uint32_t offset; ///< Offsets for List and Read commands + uint8_t data[]; ///< command data, varies by Opcode + }; + + /// @brief Command opcodes + enum Opcode : uint8_t { + kCmdNone, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in from + kCmdOpenFileRO, ///< Opens file at for reading, returns + kCmdReadFile, ///< Reads bytes from in + kCmdCreateFile, ///< Creates file at for writing, returns + kCmdWriteFile, ///< Writes bytes to in + kCmdRemoveFile, ///< Remove file at + kCmdCreateDirectory, ///< Creates directory at + kCmdRemoveDirectory, ///< Removes Directory at , must be empty + kCmdOpenFileWO, ///< Opens file at for writing, returns + kCmdTruncateFile, ///< Truncate file at to length + kCmdRename, ///< Rename to + kCmdCalcFileCRC32, ///< Calculate CRC32 for file at + kCmdBurstReadFile, ///< Burst download session file + + kRspAck = 128, ///< Ack response + kRspNak ///< Nak response + }; + + /// @brief Error codes returned in Nak response PayloadHeader.data[0]. + enum ErrorCode : uint8_t { + kErrNone, + kErrFail, ///< Unknown failure + kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrUnknownCommand, ///< Unknown command opcode + kErrFailFileExists, ///< File/directory exists already + kErrFailFileProtected, ///< File/directory is write protected + kErrFileNotFound ///< File/directory not found + }; + + unsigned get_size(); + +private: + char *_data_as_cstring(PayloadHeader *payload); + + void _process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id, uint8_t target_comp_id); + void _reply(mavlink_file_transfer_protocol_t *ftp_req); + int _copy_file(const char *src_path, const char *dst_path, size_t length); + + ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workOpen(PayloadHeader *payload, int oflag); + ErrorCode _workRead(PayloadHeader *payload); + ErrorCode _workBurst(PayloadHeader *payload, uint8_t target_system_id, uint8_t target_component_id); + ErrorCode _workWrite(PayloadHeader *payload); + ErrorCode _workTerminate(PayloadHeader *payload); + ErrorCode _workReset(PayloadHeader *payload); + ErrorCode _workRemoveDirectory(PayloadHeader *payload); + ErrorCode _workCreateDirectory(PayloadHeader *payload); + ErrorCode _workRemoveFile(PayloadHeader *payload); + ErrorCode _workTruncateFile(PayloadHeader *payload); + ErrorCode _workRename(PayloadHeader *payload); + ErrorCode _workCalcFileCRC32(PayloadHeader *payload); + + uint8_t _getServerSystemId(void); + uint8_t _getServerComponentId(void); + uint8_t _getServerChannel(void); + + /** + * make sure that the working buffers _work_buffer* are allocated + * @return true if buffers exist, false if allocation failed + */ + bool _ensure_buffers_exist(); + + static const char kDirentFile = 'F'; ///< Identifies File returned from List command + static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command + + /// @brief Maximum data size in RequestHeader::data + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); + + struct SessionInfo { + int fd; + uint32_t file_size; + bool stream_download; + uint32_t stream_offset; + uint16_t stream_seq_number; + uint8_t stream_target_system_id; + uint8_t stream_target_component_id; + unsigned stream_chunk_transmitted; + }; + struct SessionInfo _session_info {}; ///< Session info, fd=-1 for no active session + + ReceiveMessageFunc_t _utRcvMsgFunc{}; ///< Unit test override for mavlink message sending + void *_worker_data{nullptr}; ///< Additional parameter to _utRcvMsgFunc; + + Mavlink *_mavlink; + + /* do not allow copying this class */ + MavlinkFTP(const MavlinkFTP &); + MavlinkFTP operator=(const MavlinkFTP &); + + /* work buffers: they're allocated as soon as we get the first request (lazy, since FTP is rarely used) */ + char *_work_buffer1{nullptr}; + static constexpr int _work_buffer1_len = kMaxDataLength; + char *_work_buffer2{nullptr}; + static constexpr int _work_buffer2_len = 256; + hrt_abstime _last_work_buffer_access{0}; ///< timestamp when the buffers were last accessed + + // prepend a root directory to each file/dir access to avoid enumerating the full FS tree (e.g. on Linux). + // Note that requests can still fall outside of the root dir by using ../.. +#ifdef MAVLINK_FTP_UNIT_TEST + static constexpr const char _root_dir[] = ""; +#else + static constexpr const char _root_dir[] = PX4_ROOTFSDIR; +#endif + static constexpr const int _root_dir_len = sizeof(_root_dir) - 1; + + bool _last_reply_valid = false; + uint8_t _last_reply[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN - MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + + sizeof(PayloadHeader) + sizeof(uint32_t)]; + + // Mavlink test needs to be able to call send + friend class MavlinkFtpTest; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_log_handler.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_log_handler.cpp new file mode 100644 index 0000000..52611c7 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_log_handler.cpp @@ -0,0 +1,630 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 mavlink_log_handler.h +/// @author px4dev, Gus Grubba + +#include "mavlink_log_handler.h" +#include "mavlink_main.h" +#include +#include +#include + +#define MOUNTPOINT PX4_STORAGEDIR + +static const char *kLogRoot = MOUNTPOINT "/log"; +static const char *kLogData = MOUNTPOINT "/logdata.txt"; +static const char *kTmpData = MOUNTPOINT "/$log$.txt"; + +#ifdef __PX4_NUTTX +#define PX4LOG_REGULAR_FILE DTYPE_FILE +#define PX4LOG_DIRECTORY DTYPE_DIRECTORY +#else +#define PX4LOG_REGULAR_FILE DT_REG +#define PX4LOG_DIRECTORY DT_DIR +#endif + +//#define MAVLINK_LOG_HANDLER_VERBOSE + +#ifdef MAVLINK_LOG_HANDLER_VERBOSE +#define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__) +#else +#define PX4LOG_WARN(fmt, ...) +#endif + +//------------------------------------------------------------------- +static bool +stat_file(const char *file, time_t *date = nullptr, uint32_t *size = nullptr) +{ + struct stat st; + + if (stat(file, &st) == 0) { + if (date) { *date = st.st_mtime; } + + if (size) { *size = st.st_size; } + + return true; + } + + return false; +} + +//------------------------------------------------------------------- +MavlinkLogHandler::MavlinkLogHandler(Mavlink *mavlink) + : _mavlink(mavlink) +{ + +} +MavlinkLogHandler::~MavlinkLogHandler() +{ + _close_and_unlink_files(); +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_LOG_REQUEST_LIST: + _log_request_list(msg); + break; + + case MAVLINK_MSG_ID_LOG_REQUEST_DATA: + _log_request_data(msg); + break; + + case MAVLINK_MSG_ID_LOG_ERASE: + _log_request_erase(msg); + break; + + case MAVLINK_MSG_ID_LOG_REQUEST_END: + _log_request_end(msg); + break; + } +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::send() +{ + //-- An arbitrary count of max bytes in one go (one of the two below but never both) +#define MAX_BYTES_SEND 256 * 1024 + size_t count = 0; + + //-- Log Entries + while (_current_status == LogHandlerState::Listing + && _mavlink->get_free_tx_buf() > MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + && count < MAX_BYTES_SEND) { + count += _log_send_listing(); + } + + //-- Log Data + while (_current_status == LogHandlerState::SendingData + && _mavlink->get_free_tx_buf() > MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + && count < MAX_BYTES_SEND) { + count += _log_send_data(); + } +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) +{ + mavlink_log_request_list_t request; + mavlink_msg_log_request_list_decode(msg, &request); + + //-- Check for re-requests (data loss) or new request + if (_current_status != LogHandlerState::Inactive) { + //-- Is this a new request? + if ((request.end - request.start) > _log_count) { + _current_status = LogHandlerState::Inactive; + _close_and_unlink_files(); + + } else { + _current_status = LogHandlerState::Idle; + + } + } + + if (_current_status == LogHandlerState::Inactive) { + //-- Prepare new request + + _reset_list_helper(); + _init_list_helper(); + _current_status = LogHandlerState::Idle; + } + + if (_log_count) { + //-- Define (and clamp) range + _next_entry = request.start < _log_count ? request.start : + _log_count - 1; + _last_entry = request.end < _log_count ? request.end : + _log_count - 1; + } + + PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %d last: %d count: %d", + _next_entry, + _last_entry, + _log_count); + //-- Enable streaming + _current_status = LogHandlerState::Listing; +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) +{ + //-- If we haven't listed, we can't do much + if (_current_status == LogHandlerState::Inactive) { + PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested."); + return; + } + + mavlink_log_request_data_t request; + mavlink_msg_log_request_data_decode(msg, &request); + + //-- Does the requested log exist? + if (request.id >= _log_count) { + PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %" PRIu16 " but we only have %u.", request.id, + _log_count); + return; + } + + //-- If we were sending log entries, stop it + _current_status = LogHandlerState::Idle; + + if (_current_log_index != request.id) { + //-- Init send log dataset + _current_log_filename[0] = 0; + _current_log_index = request.id; + uint32_t time_utc = 0; + + if (!_get_entry(_current_log_index, _current_log_size, time_utc, + _current_log_filename, sizeof(_current_log_filename))) { + PX4LOG_WARN("LogListHelper::get_entry failed."); + return; + } + + _open_for_transmit(); + } + + _current_log_data_offset = request.ofs; + + if (_current_log_data_offset >= _current_log_size) { + _current_log_data_remaining = 0; + + } else { + _current_log_data_remaining = _current_log_size - request.ofs; + } + + if (_current_log_data_remaining > request.count) { + _current_log_data_remaining = request.count; + } + + //-- Enable streaming + _current_status = LogHandlerState::SendingData; +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_erase(const mavlink_message_t * /*msg*/) +{ + /* + mavlink_log_erase_t request; + mavlink_msg_log_erase_decode(msg, &request); + */ + _current_status = LogHandlerState::Inactive; + _close_and_unlink_files(); + + //-- Delete all logs + _delete_all(kLogRoot); +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_end(const mavlink_message_t * /*msg*/) +{ + PX4LOG_WARN("MavlinkLogHandler::_log_request_end"); + + _current_status = LogHandlerState::Inactive; + _close_and_unlink_files(); +} + +//------------------------------------------------------------------- +size_t +MavlinkLogHandler::_log_send_listing() +{ + mavlink_log_entry_t response; + uint32_t size, date; + _get_entry(_next_entry, size, date); + response.size = size; + response.time_utc = date; + response.id = _next_entry; + response.num_logs = _log_count; + response.last_log_num = _last_entry; + mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response); + + //-- If we're done listing, flag it. + if (_next_entry == _last_entry) { + _current_status = LogHandlerState::Idle; + + } else { + _next_entry++; + } + + PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %" PRIu16 " count: %" PRIu16 " last: %" PRIu16 " size: %" PRIu32 + " date: %" PRIu32 " status: %" PRIu32, + response.id, + response.num_logs, + response.last_log_num, + response.size, + response.time_utc, + (uint32_t)_current_status); + return sizeof(response); +} + +//------------------------------------------------------------------- +size_t +MavlinkLogHandler::_log_send_data() +{ + mavlink_log_data_t response; + memset(&response, 0, sizeof(response)); + uint32_t len = _current_log_data_remaining; + + if (len > sizeof(response.data)) { + len = sizeof(response.data); + } + + size_t read_size = _get_log_data(len, response.data); + response.ofs = _current_log_data_offset; + response.id = _current_log_index; + response.count = read_size; + mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response); + _current_log_data_offset += read_size; + _current_log_data_remaining -= read_size; + + if (read_size < sizeof(response.data) || _current_log_data_remaining == 0) { + _current_status = LogHandlerState::Idle; + } + + return sizeof(response); +} + +//------------------------------------------------------------------- +void MavlinkLogHandler::_close_and_unlink_files() +{ + if (_current_log_filep) { + ::fclose(_current_log_filep); + _reset_list_helper(); + } + + // Remove log data files (if any) + unlink(kLogData); + unlink(kTmpData); +} + +//------------------------------------------------------------------- +bool +MavlinkLogHandler::_get_entry(int idx, uint32_t &size, uint32_t &date, char *filename, int filename_len) +{ + //-- Find log file in log list file created during init() + size = 0; + date = 0; + bool result = false; + //-- Open list of log files + FILE *f = ::fopen(kLogData, "r"); + + if (f) { + //--- Find requested entry + char line[160]; + int count = 0; + + while (fgets(line, sizeof(line), f)) { + //-- Found our "index" + if (count++ == idx) { + char file[160]; + + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &date, &size, file) == 3) { + if (filename && filename_len > 0) { + strncpy(filename, file, filename_len); + filename[filename_len - 1] = 0; // ensure null-termination + } + + result = true; + break; + } + } + } + + fclose(f); + } + + return result; +} + +//------------------------------------------------------------------- +bool +MavlinkLogHandler::_open_for_transmit() +{ + if (_current_log_filep) { + ::fclose(_current_log_filep); + _current_log_filep = nullptr; + } + + _current_log_filep = ::fopen(_current_log_filename, "rb"); + + if (!_current_log_filep) { + PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s", _current_log_filename); + return false; + } + + return true; +} + +//------------------------------------------------------------------- +size_t +MavlinkLogHandler::_get_log_data(uint8_t len, uint8_t *buffer) +{ + if (!_current_log_filename[0]) { + return 0; + } + + if (!_current_log_filep) { + PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s", _current_log_filename); + return 0; + } + + long int offset = _current_log_data_offset - ftell(_current_log_filep); + + if (offset && fseek(_current_log_filep, offset, SEEK_CUR)) { + fclose(_current_log_filep); + _current_log_filep = nullptr; + PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s", _current_log_filename); + return 0; + } + + size_t result = fread(buffer, 1, len, _current_log_filep); + return result; +} + + +void +MavlinkLogHandler::_reset_list_helper() +{ + _next_entry = 0; + _last_entry = 0; + _log_count = 0; + _current_log_index = UINT16_MAX; + _current_log_size = 0; + _current_log_data_offset = 0; + _current_log_data_remaining = 0; + _current_log_filep = nullptr; +} + +void +MavlinkLogHandler::_init_list_helper() +{ + /* + + When this helper is created, it scans the log directory + and collects all log files found into one file for easy, + subsequent access. + */ + + _current_log_filename[0] = 0; + + // Remove old log data file (if any) + unlink(kLogData); + // Open log directory + DIR *dp = opendir(kLogRoot); + + if (dp == nullptr) { + // No log directory. Nothing to do. + return; + } + + // Create work file + FILE *f = ::fopen(kTmpData, "w"); + + if (!f) { + PX4LOG_WARN("MavlinkLogHandler::init Error creating %s", kTmpData); + closedir(dp); + return; + } + + // Scan directory and collect log files + struct dirent *result = nullptr; + + while ((result = readdir(dp))) { + if (result->d_type == PX4LOG_DIRECTORY) { + time_t tt = 0; + char log_path[128]; + int ret = snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + + if (path_is_ok) { + if (_get_session_date(log_path, result->d_name, tt)) { + _scan_logs(f, log_path, tt); + } + } + } + } + + closedir(dp); + fclose(f); + + // Rename temp file to data file + if (rename(kTmpData, kLogData)) { + PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s", kTmpData); + _log_count = 0; + } +} + +//------------------------------------------------------------------- +bool +MavlinkLogHandler::_get_session_date(const char *path, const char *dir, time_t &date) +{ + if (strlen(dir) > 4) { + // Always try to get file time first + if (stat_file(path, &date)) { + // Try to prevent taking date if it's around 1970 (use the logic below instead) + if (date > 60 * 60 * 24) { + return true; + } + } + + // Convert "sess000" to 00:00 Jan 1 1970 (day per session) + if (strncmp(dir, "sess", 4) == 0) { + unsigned u; + + if (sscanf(&dir[4], "%u", &u) == 1) { + date = u * 60 * 60 * 24; + return true; + } + } + } + + return false; +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_scan_logs(FILE *f, const char *dir, time_t &date) +{ + DIR *dp = opendir(dir); + + if (dp) { + struct dirent *result = nullptr; + + while ((result = readdir(dp))) { + if (result->d_type == PX4LOG_REGULAR_FILE) { + time_t ldate = date; + uint32_t size = 0; + char log_file_path[128]; + int ret = snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_file_path)); + + if (path_is_ok) { + if (_get_log_time_size(log_file_path, result->d_name, ldate, size)) { + //-- Write result->out to list file + fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path); + _log_count++; + } + } + } + } + + closedir(dp); + } +} + +//------------------------------------------------------------------- +bool +MavlinkLogHandler::_get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size) +{ + if (file && file[0]) { + if (strstr(file, ".px4log") || strstr(file, ".ulg")) { + // Always try to get file time first + if (stat_file(path, &date, &size)) { + // Try to prevent taking date if it's around 1970 (use the logic below instead) + if (date > 60 * 60 * 24) { + return true; + } + } + + // Convert "log000" to 00:00 (minute per flight in session) + if (strncmp(file, "log", 3) == 0) { + unsigned u; + + if (sscanf(&file[3], "%u", &u) == 1) { + date += (u * 60); + + if (stat_file(path, nullptr, &size)) { + return true; + } + } + } + } + } + + return false; +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_delete_all(const char *dir) +{ + //-- Open log directory + DIR *dp = opendir(dir); + + if (dp == nullptr) { + return; + } + + struct dirent *result = nullptr; + + while ((result = readdir(dp))) { + // no more entries? + if (result == nullptr) { + break; + } + + if (result->d_type == PX4LOG_DIRECTORY && result->d_name[0] != '.') { + char log_path[128]; + int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + + if (path_is_ok) { + _delete_all(log_path); //Recursive call. TODO: consider add protection + + if (rmdir(log_path)) { + PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s", log_path); + } + } + } + + if (result->d_type == PX4LOG_REGULAR_FILE) { + char log_path[128]; + int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + + if (path_is_ok) { + if (unlink(log_path)) { + PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s", log_path); + } + } + } + } + + closedir(dp); +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_log_handler.h b/src/prometheus_px4/src/modules/mavlink/mavlink_log_handler.h new file mode 100644 index 0000000..8993bf4 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_log_handler.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 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 + +/// @file mavlink_log_handler.h +/// @author px4dev, Gus Grubba + +#include +#include +#include +#include +#include +#include + +#include "mavlink_bridge_header.h" + +class Mavlink; + +// MAVLink LOG_* Message Handler +class MavlinkLogHandler +{ +public: + MavlinkLogHandler(Mavlink *mavlink); + ~MavlinkLogHandler(); + + // Handle possible LOG message + void handle_message(const mavlink_message_t *msg); + + /** + * Handle sending of messages. Call this regularly at a fixed frequency. + * @param t current time + */ + void send(); + + unsigned get_size(); + +private: + enum class LogHandlerState { + Inactive, //There is no active action of log handler + Idle, //The log handler is not sending list/data, but list has been sent + Listing, //File list is being send + SendingData //File Data is being send + }; + void _log_message(const mavlink_message_t *msg); + void _log_request_list(const mavlink_message_t *msg); + void _log_request_data(const mavlink_message_t *msg); + void _log_request_erase(const mavlink_message_t *msg); + void _log_request_end(const mavlink_message_t *msg); + + void _reset_list_helper(); + void _init_list_helper(); + bool _get_session_date(const char *path, const char *dir, time_t &date); + void _scan_logs(FILE *f, const char *dir, time_t &date); + bool _get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size); + static void _delete_all(const char *dir); + bool _get_entry(int idx, uint32_t &size, uint32_t &date, char *filename = 0, int filename_len = 0); + bool _open_for_transmit(); + size_t _get_log_data(uint8_t len, uint8_t *buffer); + void _close_and_unlink_files(); + + size_t _log_send_listing(); + size_t _log_send_data(); + + LogHandlerState _current_status{LogHandlerState::Inactive}; + Mavlink *_mavlink; + + int _next_entry{0}; + int _last_entry{0}; + int _log_count{0}; + + uint16_t _current_log_index{UINT16_MAX}; + uint32_t _current_log_size{0}; + uint32_t _current_log_data_offset{0}; + uint32_t _current_log_data_remaining{0}; + FILE *_current_log_filep{nullptr}; + char _current_log_filename[128]; //TODO: consider to allocate on runtime +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_main.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_main.cpp new file mode 100644 index 0000000..e599e8a --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_main.cpp @@ -0,0 +1,3166 @@ +/**************************************************************************** + * + * 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 mavlink_main.cpp + * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#include + +#ifdef CONFIG_NET +#include +#include +#include +#endif + +#include +#include +#include +#include +#include + +#include "mavlink_receiver.h" +#include "mavlink_main.h" + +// Guard against MAVLink misconfiguration +#ifndef MAVLINK_CRC_EXTRA +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + +// Guard against flow control misconfiguration +#if defined (CRTSCTS) && defined (__PX4_NUTTX) && (CRTSCTS != (CRTS_IFLOW | CCTS_OFLOW)) +#error The non-standard CRTSCTS define is incorrect. Fix this in the OS or replace with (CRTS_IFLOW | CCTS_OFLOW) +#endif + +#ifdef CONFIG_NET +#define MAVLINK_NET_ADDED_STACK 350 +#else +#define MAVLINK_NET_ADDED_STACK 0 +#endif + +#define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. +#define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s +#define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate + +static pthread_mutex_t mavlink_module_mutex = PTHREAD_MUTEX_INITIALIZER; + +Mavlink *mavlink_module_instances[MAVLINK_COMM_NUM_BUFFERS] {}; + +void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length) { mavlink_module_instances[chan]->send_bytes(ch, length); } +void mavlink_start_uart_send(mavlink_channel_t chan, int length) { mavlink_module_instances[chan]->send_start(length); } +void mavlink_end_uart_send(mavlink_channel_t chan, int length) { mavlink_module_instances[chan]->send_finish(); } +mavlink_status_t *mavlink_get_channel_status(uint8_t channel) { return mavlink_module_instances[channel]->get_status(); } +mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) { return mavlink_module_instances[channel]->get_buffer(); } + +static void usage(); + +hrt_abstime Mavlink::_first_start_time = {0}; + +bool Mavlink::_boot_complete = false; + +Mavlink::Mavlink() : + ModuleParams(nullptr), + _receiver(this) +{ + // initialise parameter cache + mavlink_update_parameters(); + + // save the current system- and component ID because we don't allow them to change during operation + int sys_id = _param_mav_sys_id.get(); + + if (sys_id > 0 && sys_id < 255) { + mavlink_system.sysid = sys_id; + } + + int comp_id = _param_mav_comp_id.get(); + + if (comp_id > 0 && comp_id < 255) { + mavlink_system.compid = comp_id; + } + + if (_first_start_time == 0) { + _first_start_time = hrt_absolute_time(); + } + + // ensure topic exists, otherwise we might lose first queued commands + if (orb_exists(ORB_ID(vehicle_command), 0) == PX4_ERROR) { + orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH); + } + + _vehicle_command_sub.subscribe(); +} + +Mavlink::~Mavlink() +{ + if (_task_running) { + /* task wakes up every 10ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + px4_usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + //TODO store main task handle in Mavlink instance to allow killing task + //task_delete(_mavlink_task); + break; + } + } while (_task_running); + } + + if (_instance_id >= 0) { + mavlink_module_instances[_instance_id] = nullptr; + } + + perf_free(_loop_perf); + perf_free(_loop_interval_perf); + perf_free(_send_byte_error_perf); +} + +void +Mavlink::mavlink_update_parameters() +{ + updateParams(); + + int32_t proto = _param_mav_proto_ver.get(); + + if (_protocol_version_switch != proto) { + _protocol_version_switch = proto; + set_proto_version(proto); + } + + if (_param_mav_type.get() < 0 || _param_mav_type.get() >= MAV_TYPE_ENUM_END) { + _param_mav_type.set(0); + _param_mav_type.commit_no_notification(); + PX4_ERR("MAV_TYPE parameter invalid, resetting to 0."); + } +} + +void +Mavlink::set_channel() +{ + /* set channel according to instance id */ + switch (_instance_id) { + case 0: + _channel = MAVLINK_COMM_0; + break; + + case 1: + _channel = MAVLINK_COMM_1; + break; + + case 2: + _channel = MAVLINK_COMM_2; + break; + + case 3: + _channel = MAVLINK_COMM_3; + break; +#ifdef MAVLINK_COMM_4 + + case 4: + _channel = MAVLINK_COMM_4; + break; +#endif +#ifdef MAVLINK_COMM_5 + + case 5: + _channel = MAVLINK_COMM_5; + break; +#endif +#ifdef MAVLINK_COMM_6 + + case 6: + _channel = MAVLINK_COMM_6; + break; +#endif + + default: + PX4_WARN("instance ID %d is out of range", _instance_id); + px4_task_exit(1); + break; + } +} + +bool +Mavlink::set_instance_id() +{ + LockGuard lg{mavlink_module_mutex}; + + for (int instance_id = 0; instance_id < MAVLINK_COMM_NUM_BUFFERS; instance_id++) { + if (mavlink_module_instances[instance_id] == nullptr) { + mavlink_module_instances[instance_id] = this; + _instance_id = instance_id; + return true; + } + } + + return false; +} + +void +Mavlink::set_proto_version(unsigned version) +{ + if ((version == 1 || version == 0) && + ((_protocol_version_switch == 0) || (_protocol_version_switch == 1))) { + get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + _protocol_version = 1; + + } else if (version == 2 && + ((_protocol_version_switch == 0) || (_protocol_version_switch == 2))) { + get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + _protocol_version = 2; + } +} + +int +Mavlink::instance_count() +{ + LockGuard lg{mavlink_module_mutex}; + size_t inst_index = 0; + + for (Mavlink *inst : mavlink_module_instances) { + if (inst != nullptr) { + inst_index++; + } + } + + return inst_index; +} + +Mavlink * +Mavlink::get_instance_for_device(const char *device_name) +{ + LockGuard lg{mavlink_module_mutex}; + + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst->_protocol == Protocol::SERIAL) && (strcmp(inst->_device_name, device_name) == 0)) { + return inst; + } + } + + return nullptr; +} + +#ifdef MAVLINK_UDP +Mavlink * +Mavlink::get_instance_for_network_port(unsigned long port) +{ + LockGuard lg{mavlink_module_mutex}; + + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst->_protocol == Protocol::UDP) && (inst->_network_port == port)) { + return inst; + } + } + + return nullptr; +} +#endif // MAVLINK_UDP + +int +Mavlink::destroy_all_instances() +{ + LockGuard lg{mavlink_module_mutex}; + unsigned iterations = 0; + + PX4_INFO("waiting for instances to stop"); + + for (Mavlink *inst_to_del : mavlink_module_instances) { + if (inst_to_del != nullptr) { + /* set flag to stop thread and wait for all threads to finish */ + inst_to_del->_task_should_exit = true; + + while (inst_to_del->_task_running) { + printf("."); + fflush(stdout); + px4_usleep(10000); + iterations++; + + if (iterations > 1000) { + PX4_ERR("Couldn't stop all mavlink instances."); + return PX4_ERROR; + } + } + } + } + + //we know all threads have exited, so it's safe to delete objects. + for (Mavlink *inst_to_del : mavlink_module_instances) { + delete inst_to_del; + } + + printf("\n"); + PX4_INFO("all instances stopped"); + return OK; +} + +int +Mavlink::get_status_all_instances(bool show_streams_status) +{ + LockGuard lg{mavlink_module_mutex}; + unsigned iterations = 0; + + for (Mavlink *inst : mavlink_module_instances) { + if (inst != nullptr) { + printf("\ninstance #%u:\n", iterations); + + if (show_streams_status) { + inst->display_status_streams(); + + } else { + inst->display_status(); + } + + iterations++; + } + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + +bool +Mavlink::serial_instance_exists(const char *device_name, Mavlink *self) +{ + LockGuard lg{mavlink_module_mutex}; + + for (Mavlink *inst : mavlink_module_instances) { + /* don't compare with itself and with non serial instances*/ + if (inst && (inst != self) && (inst->get_protocol() == Protocol::SERIAL) && !strcmp(device_name, inst->_device_name)) { + return true; + } + } + + return false; +} + +void +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) +{ + LockGuard lg{mavlink_module_mutex}; + + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst != self)) { + const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid); + + int target_system_id = 0; + int target_component_id = 0; + + // might be nullptr if message is unknown + if (meta) { + // Extract target system and target component if set + if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { + if (meta->target_system_ofs < msg->len) { + target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs]; + } + } + + if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { + if (meta->target_component_ofs < msg->len) { + target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs]; + } + } + } + + // If it's a message only for us, we keep it, otherwise, we forward it. + const bool targeted_only_at_us = + (target_system_id == self->get_system_id() && + target_component_id == self->get_component_id()); + + // We don't forward heartbeats unless it's specifically enabled. + const bool heartbeat_check_ok = + (msg->msgid != MAVLINK_MSG_ID_HEARTBEAT || self->forward_heartbeats_enabled()); + + if (!targeted_only_at_us && heartbeat_check_ok) { + inst->pass_message(msg); + } + } + } +} + +int +Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CONTROL_MODE flow_control) +{ +#ifndef B460800 +#define B460800 460800 +#endif + +#ifndef B500000 +#define B500000 500000 +#endif + +#ifndef B921600 +#define B921600 921600 +#endif + +#ifndef B1000000 +#define B1000000 1000000 +#endif + + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 500000: speed = B500000; break; + + case 921600: speed = B921600; break; + + case 1000000: speed = B1000000; break; + +#ifdef B1500000 + + case 1500000: speed = B1500000; break; +#endif + +#ifdef B2000000 + + case 2000000: speed = B2000000; break; +#endif + +#ifdef B3000000 + + case 3000000: speed = B3000000; break; +#endif + + default: + PX4_ERR("Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n500000\n921600\n1000000\n", + baud); + return -EINVAL; + } + + /* back off 1800 ms to avoid running into the USB setup timing */ + while (_is_usb_uart && hrt_absolute_time() < 1800U * 1000U) { + px4_usleep(50000); + } + + /* open uart */ + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + + /* if this is a config link, stay here and wait for it to open */ + if (_uart_fd < 0 && _is_usb_uart) { + + uORB::SubscriptionData armed_sub{ORB_ID(actuator_armed)}; + + /* get the system arming state and abort on arming */ + while (_uart_fd < 0 && !_task_should_exit) { + + /* another task might have requested subscriptions: make sure we handle it */ + check_requested_subscriptions(); + + /* abort if an arming topic is published and system is armed */ + armed_sub.update(); + + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + if (armed_sub.get().armed) { + /* this is not an error, but we are done */ + return -1; + } + + int errcode = errno; + /* ENOTCONN means that the USB device is not yet connected */ + px4_usleep(errcode == ENOTCONN ? 1000000 : 100000); + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + } + } + + /* + * Return here in the iridium mode since the iridium driver does not + * support the subsequent function calls. + */ + if (_uart_fd < 0 || _mode == MAVLINK_MODE_IRIDIUM) { + return _uart_fd; + } + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Initialize the uart config */ + if ((termios_state = tcgetattr(_uart_fd, &uart_config)) < 0) { + PX4_ERR("ERR GET CONF %s: %d\n", uart_name, termios_state); + ::close(_uart_fd); + return -1; + } + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + if (!_is_usb_uart) { + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state); + ::close(_uart_fd); + return -1; + } + + } else { + + /* USB has no baudrate, but use a magic number for 'fast' */ + _baudrate = 2000000; + + set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB); + } + +#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) + /* Put in raw mode */ + cfmakeraw(&uart_config); +#endif + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + PX4_WARN("ERR SET CONF %s\n", uart_name); + ::close(_uart_fd); + return -1; + } + + /* setup hardware flow control */ + if (setup_flow_control(flow_control) && (flow_control != FLOW_CONTROL_AUTO)) { + PX4_WARN("hardware flow control not supported"); + } + + return _uart_fd; +} + +int +Mavlink::setup_flow_control(enum FLOW_CONTROL_MODE mode) +{ + // We can't do this on USB - skip + if (_is_usb_uart) { + _flow_control_mode = FLOW_CONTROL_OFF; + return OK; + } + + struct termios uart_config; + + int ret = tcgetattr(_uart_fd, &uart_config); + + if (mode != FLOW_CONTROL_OFF) { + uart_config.c_cflag |= CRTSCTS; + + } else { + uart_config.c_cflag &= ~CRTSCTS; + + } + + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + + if (!ret) { + _flow_control_mode = mode; + } + + return ret; +} + +int +Mavlink::set_hil_enabled(bool hil_enabled) +{ + int ret = OK; + + /* enable HIL (only on links with sufficient bandwidth) */ + if (hil_enabled && !_hil_enabled && _datarate > 5000) { + _hil_enabled = true; + ret = configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f); + + if (_param_sys_hitl.get() == 2) { // Simulation in Hardware enabled ? + configure_stream("HIL_STATE_QUATERNION", 25.0f); // ground truth to display the SIH + + } else { + configure_stream("HIL_STATE_QUATERNION", 0.0f); + } + } + + /* disable HIL */ + if (!hil_enabled && _hil_enabled) { + _hil_enabled = false; + ret = configure_stream("HIL_ACTUATOR_CONTROLS", 0.0f); + + configure_stream("HIL_STATE_QUATERNION", 0.0f); + } + + return ret; +} + +unsigned +Mavlink::get_free_tx_buf() +{ + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + +#if defined(MAVLINK_UDP) + + // if we are using network sockets, return max length of one packet + if (get_protocol() == Protocol::UDP) { +# if defined(__PX4_POSIX) + return 1500 * 10; // Speed up FTP transfers +# else + return 1500; +# endif /* defined(__PX4_POSIX) */ + + } else +#endif // MAVLINK_UDP + { + +#if defined(__PX4_NUTTX) + (void) ioctl(_uart_fd, FIONSPACE, (unsigned long)&buf_free); +#else + // No FIONSPACE on Linux todo:use SIOCOUTQ and queue size to emulate FIONSPACE + //Linux cp210x does not support TIOCOUTQ + buf_free = MAVLINK_MAX_PACKET_LEN; +#endif + + if (_flow_control_mode == FLOW_CONTROL_AUTO && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { + /* Disable hardware flow control in FLOW_CONTROL_AUTO mode: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500_ms && + _last_write_success_time != _last_write_try_time) { + + setup_flow_control(FLOW_CONTROL_OFF); + } + } + } + + return buf_free; +} + +void Mavlink::send_start(int length) +{ + pthread_mutex_lock(&_send_mutex); + _last_write_try_time = hrt_absolute_time(); + + // check if there is space in the buffer + if (length > (int)get_free_tx_buf()) { + // not enough space in buffer to send + count_txerrbytes(length); + + _tstatus.tx_buffer_overruns++; + + // prevent writes + _tx_buffer_low = true; + + } else { + _tx_buffer_low = false; + } +} + +void Mavlink::send_finish() +{ + if (_tx_buffer_low || (_buf_fill == 0)) { + pthread_mutex_unlock(&_send_mutex); + return; + } + + int ret = -1; + + // send message to UART + if (get_protocol() == Protocol::SERIAL) { + ret = ::write(_uart_fd, _buf, _buf_fill); + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + +# if defined(CONFIG_NET) + + if (_src_addr_initialized) { +# endif // CONFIG_NET + ret = sendto(_socket_fd, _buf, _buf_fill, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); +# if defined(CONFIG_NET) + } + +# endif // CONFIG_NET + + if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() && + (!get_client_source_initialized() || !is_connected())) { + + if (!_broadcast_address_found) { + find_broadcast_address(); + } + + if (_broadcast_address_found && _buf_fill > 0) { + + int bret = sendto(_socket_fd, _buf, _buf_fill, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr)); + + if (bret <= 0) { + if (!_broadcast_failed_warned) { + PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno)); + _broadcast_failed_warned = true; + } + + } else { + _broadcast_failed_warned = false; + } + } + } + } + +#endif // MAVLINK_UDP + + if (ret == (int)_buf_fill) { + _tstatus.tx_message_count++; + count_txbytes(_buf_fill); + _last_write_success_time = _last_write_try_time; + + } else { + count_txerrbytes(_buf_fill); + } + + _buf_fill = 0; + + pthread_mutex_unlock(&_send_mutex); +} + +void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) +{ + if (!_tx_buffer_low) { + if (_buf_fill + packet_len < sizeof(_buf)) { + memcpy(&_buf[_buf_fill], buf, packet_len); + _buf_fill += packet_len; + + } else { + perf_count(_send_byte_error_perf); + } + } +} + +#ifdef MAVLINK_UDP +void Mavlink::find_broadcast_address() +{ +#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) + struct ifconf ifconf; + int ret; + +#if defined(__APPLE__) && defined(__MACH__) || defined(__CYGWIN__) + // On Mac, we can't determine the required buffer + // size in advance, so we just use what tends to work. + ifconf.ifc_len = 1024; +#else + // On Linux, we can determine the required size of the + // buffer first by providing NULL to ifc_req. + ifconf.ifc_req = nullptr; + ifconf.ifc_len = 0; + + ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf); + + if (ret != 0) { + PX4_WARN("getting required buffer size failed"); + return; + } + +#endif + + PX4_DEBUG("need to allocate %d bytes", ifconf.ifc_len); + + // Allocate buffer. + ifconf.ifc_req = (struct ifreq *)(new uint8_t[ifconf.ifc_len]); + + if (ifconf.ifc_req == nullptr) { + PX4_ERR("Could not allocate ifconf buffer"); + return; + } + + memset(ifconf.ifc_req, 0, ifconf.ifc_len); + + ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf); + + if (ret != 0) { + PX4_ERR("getting network config failed"); + delete[] ifconf.ifc_req; + return; + } + + int offset = 0; + // Later used to point to next network interface in buffer. + struct ifreq *cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]); + + // The ugly `for` construct is used because it allows to use + // `continue` and `break`. + for (; + offset < ifconf.ifc_len; +#if defined(__APPLE__) && defined(__MACH__) + // On Mac, to get to next entry in buffer, jump by the size of + // the interface name size plus whatever is greater, either the + // sizeof sockaddr or ifr_addr.sa_len. + offset += IF_NAMESIZE + + (sizeof(struct sockaddr) > cur_ifreq->ifr_addr.sa_len ? + sizeof(struct sockaddr) : cur_ifreq->ifr_addr.sa_len) +#else + // On Linux, it's much easier to traverse the buffer, every entry + // has the constant length. + offset += sizeof(struct ifreq) +#endif + ) { + // Point to next network interface in buffer. + cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]); + + PX4_DEBUG("looking at %s", cur_ifreq->ifr_name); + + // ignore loopback network + if (strcmp(cur_ifreq->ifr_name, "lo") == 0 || + strcmp(cur_ifreq->ifr_name, "lo0") == 0 || + strcmp(cur_ifreq->ifr_name, "lo1") == 0 || + strcmp(cur_ifreq->ifr_name, "lo2") == 0) { + PX4_DEBUG("skipping loopback"); + continue; + } + + struct in_addr &sin_addr = ((struct sockaddr_in *)&cur_ifreq->ifr_addr)->sin_addr; + + // Accept network interfaces to local network only. This means it's an IP starting with: + // 192./172./10. + // Also see https://tools.ietf.org/html/rfc1918#section-3 + + uint8_t first_byte = sin_addr.s_addr & 0xFF; + + if (first_byte != 192 && first_byte != 172 && first_byte != 10) { + continue; + } + + if (!_broadcast_address_found) { + const struct in_addr netmask_addr = query_netmask_addr(_socket_fd, *cur_ifreq); + const struct in_addr broadcast_addr = compute_broadcast_addr(sin_addr, netmask_addr); + + if (_interface_name && strstr(cur_ifreq->ifr_name, _interface_name) == nullptr) { continue; } + + PX4_INFO("using network interface %s, IP: %s", cur_ifreq->ifr_name, inet_ntoa(sin_addr)); + PX4_INFO("with netmask: %s", inet_ntoa(netmask_addr)); + PX4_INFO("and broadcast IP: %s", inet_ntoa(broadcast_addr)); + + _bcast_addr.sin_family = AF_INET; + _bcast_addr.sin_addr = broadcast_addr; + + _broadcast_address_found = true; + + } else { + PX4_DEBUG("ignoring additional network interface %s, IP: %s", + cur_ifreq->ifr_name, inet_ntoa(sin_addr)); + } + } + +#elif defined (CONFIG_NET) && defined (__PX4_NUTTX) + int ret; + + PX4_INFO("using network interface"); + + struct in_addr eth_addr; + struct in_addr bc_addr; + struct in_addr netmask_addr; + ret = netlib_get_ipv4addr("eth0", ð_addr); + + if (ret != 0) { + PX4_ERR("getting network config failed"); + return; + } + + ret = netlib_get_ipv4netmask("eth0", &netmask_addr); + + if (ret != 0) { + PX4_ERR("getting network config failed"); + return; + } + + PX4_INFO("ipv4addr IP: %s", inet_ntoa(eth_addr)); + PX4_INFO("netmask_addr IP: %s", inet_ntoa(netmask_addr)); + + bc_addr.s_addr = eth_addr.s_addr | ~(netmask_addr.s_addr); + + if (!_broadcast_address_found) { + PX4_INFO("using network interface %s, IP: %s", "eth0", inet_ntoa(eth_addr)); + + //struct in_addr &bc_addr = ((struct sockaddr_in *)&bc_ifreq.ifr_broadaddr)->sin_addr; + PX4_INFO("with broadcast IP: %s", inet_ntoa(bc_addr)); + + _bcast_addr.sin_family = AF_INET; + _bcast_addr.sin_addr = bc_addr; + + _broadcast_address_found = true; + } + +#endif + +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) + + if (_broadcast_address_found) { + _bcast_addr.sin_port = htons(_remote_port); + + int broadcast_opt = 1; + + if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) { + PX4_WARN("setting broadcast permission failed"); + } + + _broadcast_address_not_found_warned = false; + + } else { + if (!_broadcast_address_not_found_warned) { + PX4_WARN("no broadcasting address found"); + _broadcast_address_not_found_warned = true; + } + } + +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) + delete[] ifconf.ifc_req; +#endif + +#endif +} +#endif // MAVLINK_UDP + +#ifdef __PX4_POSIX +const in_addr +Mavlink::query_netmask_addr(const int socket_fd, const ifreq &ifreq) +{ + struct ifreq netmask_ifreq; + memset(&netmask_ifreq, 0, sizeof(netmask_ifreq)); + strncpy(netmask_ifreq.ifr_name, ifreq.ifr_name, IF_NAMESIZE); + ioctl(socket_fd, SIOCGIFNETMASK, &netmask_ifreq); + + return ((struct sockaddr_in *)&netmask_ifreq.ifr_addr)->sin_addr; +} + +const in_addr +Mavlink::compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask_addr) +{ + struct in_addr broadcast_addr; + broadcast_addr.s_addr = ~netmask_addr.s_addr | host_addr.s_addr; + + return broadcast_addr; +} +#endif + +#ifdef MAVLINK_UDP +void +Mavlink::init_udp() +{ + PX4_DEBUG("Setting up UDP with port %hu", _network_port); + + _myaddr.sin_family = AF_INET; + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + _myaddr.sin_port = htons(_network_port); + + if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_WARN("create socket failed: %s", strerror(errno)); + return; + } + + if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + PX4_WARN("bind failed: %s", strerror(errno)); + return; + } + + /* set default target address, but not for onboard mode (will be set on first received packet) */ + if (!_src_addr_initialized) { + _src_addr.sin_family = AF_INET; + inet_aton("127.0.0.1", &_src_addr.sin_addr); + } + + _src_addr.sin_port = htons(_remote_port); +} +#endif // MAVLINK_UDP + +void +Mavlink::handle_message(const mavlink_message_t *msg) +{ + /* + * NOTE: this is called from the receiver thread + */ + + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); + } + + // Special case for gimbals that need to forward GIMBAL_DEVICE_ATTITUDE_STATUS. + else if (msg->msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS) { + Mavlink::forward_message(msg, this); + } +} + +void +Mavlink::send_statustext_info(const char *string) +{ + mavlink_log_info(&_mavlink_log_pub, "%s", string); +} + +void +Mavlink::send_statustext_critical(const char *string) +{ + mavlink_log_critical(&_mavlink_log_pub, "%s", string); +} + +void +Mavlink::send_statustext_emergency(const char *string) +{ + mavlink_log_emergency(&_mavlink_log_pub, "%s", string); +} + +bool +Mavlink::send_autopilot_capabilities() +{ + uORB::Subscription status_sub{ORB_ID(vehicle_status)}; + vehicle_status_s status; + + if (status_sub.copy(&status)) { + mavlink_autopilot_version_t msg{}; + + msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY; + msg.flight_sw_version = px4_firmware_version(); + msg.middleware_sw_version = px4_firmware_version(); + msg.os_sw_version = px4_os_version(); + msg.board_version = px4_board_version(); + /* use only first 5 bytes of git hash for firmware version */ + const uint64_t fw_git_version_binary = px4_firmware_version_binary() & 0xFFFFFFFFFF000000; + const uint64_t fw_vendor_version = px4_firmware_vendor_version() >> 8; + constexpr size_t fw_vendor_version_length = 3; + memcpy(&msg.flight_custom_version, &fw_git_version_binary, sizeof(msg.flight_custom_version)); + memcpy(&msg.flight_custom_version, &fw_vendor_version, fw_vendor_version_length); + memcpy(&msg.middleware_custom_version, &fw_git_version_binary, sizeof(msg.middleware_custom_version)); + uint64_t os_git_version_binary = px4_os_version_binary(); + memcpy(&msg.os_custom_version, &os_git_version_binary, sizeof(msg.os_custom_version)); +#ifdef CONFIG_CDCACM_VENDORID + msg.vendor_id = CONFIG_CDCACM_VENDORID; +#else + msg.vendor_id = 0; +#endif +#ifdef CONFIG_CDCACM_PRODUCTID + msg.product_id = CONFIG_CDCACM_PRODUCTID; +#else + msg.product_id = 0; +#endif + uuid_uint32_t uid; + board_get_uuid32(uid); + msg.uid = (((uint64_t)uid[PX4_CPU_UUID_WORD32_UNIQUE_M]) << 32) | uid[PX4_CPU_UUID_WORD32_UNIQUE_H]; + +#ifndef BOARD_HAS_NO_UUID + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + static_assert(sizeof(px4_guid_t) == sizeof(msg.uid2), "GUID byte length mismatch"); + memcpy(&msg.uid2, &px4_guid, sizeof(msg.uid2)); +#endif /* BOARD_HAS_NO_UUID */ + +#ifdef CONFIG_ARCH_BOARD_PX4_SITL + // To avoid that multiple SITL instances have the same UUID, we add the mavlink + // system ID. We subtract 1, so that the first UUID remains unchanged given the + // default system ID is 1. + // + // Note that the UUID show in `ver` will still be the same for all instances. + msg.uid += mavlink_system.sysid - 1; + msg.uid2[0] += mavlink_system.sysid - 1; +#endif /* CONFIG_ARCH_BOARD_PX4_SITL */ + mavlink_msg_autopilot_version_send_struct(get_channel(), &msg); + return true; + } + + return false; +} + +void +Mavlink::send_protocol_version() +{ + mavlink_protocol_version_t msg = {}; + + msg.version = _protocol_version * 100; + msg.min_version = 100; + msg.max_version = 200; + uint64_t mavlink_lib_git_version_binary = px4_mavlink_lib_version_binary(); + // TODO add when available + //memcpy(&msg.spec_version_hash, &mavlink_spec_git_version_binary, sizeof(msg.spec_version_hash)); + memcpy(&msg.library_version_hash, &mavlink_lib_git_version_binary, sizeof(msg.library_version_hash)); + + // Switch to MAVLink 2 + int curr_proto_ver = _protocol_version; + set_proto_version(2); + // Send response - if it passes through the link its fine to use MAVLink 2 + mavlink_msg_protocol_version_send_struct(get_channel(), &msg); + // Reset to previous value + set_proto_version(curr_proto_ver); +} + +int +Mavlink::configure_stream(const char *stream_name, const float rate) +{ + PX4_DEBUG("configure_stream(%s, %.3f)", stream_name, (double)rate); + + /* calculate interval in us, -1 means unlimited stream, 0 means disabled */ + int interval = 0; + + if (rate > 0.000001f) { + interval = (1000000.0f / rate); + + } else if (rate < 0.0f) { + interval = -1; + } + + for (const auto &stream : _streams) { + if (strcmp(stream_name, stream->get_name()) == 0) { + if (interval != 0) { + /* set new interval */ + stream->set_interval(interval); + + } else { + /* delete stream */ + _streams.deleteNode(stream); + return OK; // must finish with loop after node is deleted + } + + return OK; + } + } + + // search for stream with specified name in supported streams list + // create new instance if found + MavlinkStream *stream = create_mavlink_stream(stream_name, this); + + if (stream != nullptr) { + stream->set_interval(interval); + _streams.add(stream); + + return OK; + } + + /* if we reach here, the stream list does not contain the stream */ +#if defined(CONSTRAINED_FLASH) // flash constrained target's don't include all streams + return PX4_OK; +#else + PX4_WARN("stream %s not found", stream_name); + return PX4_ERROR; +#endif +} + +void +Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) +{ + /* orb subscription must be done from the main thread, + * set _subscribe_to_stream and _subscribe_to_stream_rate fields + * which polled in mavlink main loop */ + if (!_task_should_exit) { + /* wait for previous subscription completion */ + while (_subscribe_to_stream != nullptr) { + px4_usleep(MAIN_LOOP_DELAY / 2); + } + + /* copy stream name */ + unsigned n = strlen(stream_name) + 1; + char *s = new char[n]; + strcpy(s, stream_name); + + /* set subscription task */ + _subscribe_to_stream_rate = rate; + _subscribe_to_stream = s; + + /* wait for subscription */ + do { + px4_usleep(MAIN_LOOP_DELAY / 2); + } while (_subscribe_to_stream != nullptr); + + delete[] s; + } +} + +int +Mavlink::message_buffer_init(int size) +{ + _message_buffer.size = size; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + _message_buffer.data = (char *)malloc(_message_buffer.size); + + int ret; + + if (_message_buffer.data == nullptr) { + ret = PX4_ERROR; + _message_buffer.size = 0; + + } else { + ret = OK; + } + + return ret; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +bool +Mavlink::message_buffer_write(const void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::pass_message(const mavlink_message_t *msg) +{ + if (_forwarding_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + +MavlinkShell * +Mavlink::get_shell() +{ + if (!_mavlink_shell) { + _mavlink_shell = new MavlinkShell(); + + if (!_mavlink_shell) { + PX4_ERR("Failed to allocate a shell"); + + } else { + int ret = _mavlink_shell->start(); + + if (ret != 0) { + PX4_ERR("Failed to start shell (%i)", ret); + delete _mavlink_shell; + _mavlink_shell = nullptr; + } + } + } + + return _mavlink_shell; +} + +void +Mavlink::close_shell() +{ + if (_mavlink_shell) { + delete _mavlink_shell; + _mavlink_shell = nullptr; + } +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + /* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */ + for (const auto &stream : _streams) { + if (stream->const_rate()) { + const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0; + + } else { + rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0; + } + } + + float mavlink_ulog_streaming_rate_inv = 1.0f; + + if (_mavlink_ulog) { + mavlink_ulog_streaming_rate_inv = 1.0f - _mavlink_ulog->current_data_rate(); + } + + /* scale up and down as the link permits */ + float bandwidth_mult = (float)(_datarate * mavlink_ulog_streaming_rate_inv - const_rate) / rate; + + /* if we do not have flow control, limit to the set data rate */ + if (!get_flow_control_enabled()) { + bandwidth_mult = fminf(1.0f, bandwidth_mult); + } + + float hardware_mult = 1.0f; + + // scale down if we have a TX err rate suggesting link congestion + if ((_tstatus.tx_error_rate_avg > 0.f) && !_radio_status_critical) { + hardware_mult = _tstatus.tx_rate_avg / (_tstatus.tx_rate_avg + _tstatus.tx_error_rate_avg); + + } else if (_radio_status_available) { + + // check for RADIO_STATUS timeout and reset + if (hrt_elapsed_time(&_rstatus.timestamp) > (_param_mav_radio_timeout.get() * 1_s)) { + PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); + _radio_status_available = false; + + if (_use_software_mav_throttling) { + _radio_status_critical = false; + _radio_status_mult = 1.0f; + } + } + + hardware_mult *= _radio_status_mult; + } + + /* pick the minimum from bandwidth mult and hardware mult as limit */ + _rate_mult = fminf(bandwidth_mult, hardware_mult); + + /* ensure the rate multiplier never drops below 5% so that something is always sent */ + _rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f); +} + +void +Mavlink::update_radio_status(const radio_status_s &radio_status) +{ + _rstatus = radio_status; + _radio_status_available = true; + + if (_use_software_mav_throttling) { + + /* check hardware limits */ + _radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE); + + if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) { + /* this indicates link congestion, reduce rate by 20% */ + _radio_status_mult *= 0.80f; + + } else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) { + /* this indicates link congestion, reduce rate by 2.5% */ + _radio_status_mult *= 0.975f; + + } else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) { + /* this indicates spare bandwidth, increase by 2.5% */ + _radio_status_mult *= 1.025f; + } + + /* Constrain radio status multiplier between 1% and 100% to allow recovery */ + _radio_status_mult = math::constrain(_radio_status_mult, 0.01f, 1.0f); + } +} + +int +Mavlink::configure_streams_to_default(const char *configure_single_stream) +{ + int ret = 0; + bool stream_configured = false; + + auto configure_stream_local = + [&stream_configured, configure_single_stream, &ret, this](const char *stream_name, float rate) { + if (!configure_single_stream || strcmp(configure_single_stream, stream_name) == 0) { + int ret_local = configure_stream(stream_name, rate); + + if (ret_local != 0) { + ret = ret_local; + } + + stream_configured = true; + } + }; + + const float unlimited_rate = -1.0f; + + switch (_mode) { + case MAVLINK_MODE_NORMAL: + configure_stream_local("ADSB_VEHICLE", unlimited_rate); + configure_stream_local("ALTITUDE", 1.0f); + configure_stream_local("ATTITUDE", 15.0f); + configure_stream_local("ATTITUDE_TARGET", 2.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); + configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); + configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("DISTANCE_SENSOR", 0.5f); + configure_stream_local("ESC_INFO", 1.0f); + configure_stream_local("ESC_STATUS", 1.0f); + configure_stream_local("ESTIMATOR_STATUS", 0.5f); + configure_stream_local("EXTENDED_SYS_STATE", 1.0f); + configure_stream_local("GLOBAL_POSITION_INT", 5.0f); + configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); + configure_stream_local("GPS2_RAW", 1.0f); + configure_stream_local("GPS_GLOBAL_ORIGIN", 0.1f); + configure_stream_local("GPS_RAW_INT", 1.0f); + configure_stream_local("GPS_STATUS", 1.0f); + configure_stream_local("HOME_POSITION", 0.5f); + configure_stream_local("LOCAL_POSITION_NED", 1.0f); + configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f); + configure_stream_local("OBSTACLE_DISTANCE", 1.0f); + configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f); + configure_stream_local("PING", 0.1f); + configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.0f); + configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); + configure_stream_local("RAW_RPM", 2.0f); + configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); + configure_stream_local("SYS_STATUS", 1.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 0.5f); + configure_stream_local("VFR_HUD", 4.0f); + configure_stream_local("VIBRATION", 0.1f); + configure_stream_local("WIND_COV", 0.5f); + +#if !defined(CONSTRAINED_FLASH) + configure_stream_local("DEBUG", 1.0f); + configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f); + configure_stream_local("DEBUG_VECT", 1.0f); + configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); + configure_stream_local("LINK_NODE_STATUS", 1.0f); +#endif // !CONSTRAINED_FLASH + + break; + + case MAVLINK_MODE_ONBOARD: + // Note: streams requiring low latency come first + configure_stream_local("TIMESYNC", 10.0f); + configure_stream_local("CAMERA_TRIGGER", unlimited_rate); + configure_stream_local("HIGHRES_IMU", 50.0f); + configure_stream_local("LOCAL_POSITION_NED", 50.0f); + configure_stream_local("ATTITUDE", 100.0f); + configure_stream_local("ALTITUDE", 10.0f); + configure_stream_local("DISTANCE_SENSOR", 10.0f); + configure_stream_local("ESC_INFO", 10.0f); + configure_stream_local("ESC_STATUS", 10.0f); + configure_stream_local("MOUNT_ORIENTATION", 10.0f); + configure_stream_local("OBSTACLE_DISTANCE", 10.0f); + configure_stream_local("ODOMETRY", 30.0f); + + configure_stream_local("ACTUATOR_CONTROL_TARGET0", 10.0f); + configure_stream_local("ADSB_VEHICLE", unlimited_rate); + configure_stream_local("ATTITUDE_QUATERNION", 50.0f); + configure_stream_local("ATTITUDE_TARGET", 10.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); + configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); + configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("ESTIMATOR_STATUS", 1.0f); + configure_stream_local("EXTENDED_SYS_STATE", 5.0f); + configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); + configure_stream_local("GLOBAL_POSITION_INT", 50.0f); + configure_stream_local("GPS2_RAW", unlimited_rate); + configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); + configure_stream_local("GPS_RAW_INT", unlimited_rate); + configure_stream_local("GPS_STATUS", 1.0f); + configure_stream_local("HOME_POSITION", 0.5f); + configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); + configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); + configure_stream_local("PING", 1.0f); + configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f); + configure_stream_local("RAW_RPM", 5.0f); + configure_stream_local("RC_CHANNELS", 20.0f); + configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f); + configure_stream_local("SYS_STATUS", 5.0f); + configure_stream_local("SYSTEM_TIME", 1.0f); + configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); + configure_stream_local("VFR_HUD", 10.0f); + configure_stream_local("VIBRATION", 0.5f); + configure_stream_local("WIND_COV", 10.0f); + +#if !defined(CONSTRAINED_FLASH) + configure_stream_local("DEBUG", 10.0f); + configure_stream_local("DEBUG_FLOAT_ARRAY", 10.0f); + configure_stream_local("DEBUG_VECT", 10.0f); + configure_stream_local("NAMED_VALUE_FLOAT", 10.0f); + configure_stream_local("LINK_NODE_STATUS", 1.0f); +#endif // !CONSTRAINED_FLASH + + break; + + case MAVLINK_MODE_GIMBAL: + // Note: streams requiring low latency come first + configure_stream_local("AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", 20.0f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 20.0f); + break; + + case MAVLINK_MODE_EXTVISION: + configure_stream_local("HIGHRES_IMU", unlimited_rate); // for VIO + + // FALLTHROUGH + case MAVLINK_MODE_EXTVISIONMIN: + // Note: streams requiring low latency come first + configure_stream_local("TIMESYNC", 10.0f); + configure_stream_local("CAMERA_TRIGGER", unlimited_rate); + configure_stream_local("LOCAL_POSITION_NED", 30.0f); + configure_stream_local("ATTITUDE", 20.0f); + configure_stream_local("ALTITUDE", 10.0f); + configure_stream_local("DISTANCE_SENSOR", 10.0f); + configure_stream_local("MOUNT_ORIENTATION", 10.0f); + configure_stream_local("OBSTACLE_DISTANCE", 10.0f); + configure_stream_local("ODOMETRY", 30.0f); + + configure_stream_local("ADSB_VEHICLE", unlimited_rate); + configure_stream_local("ATTITUDE_TARGET", 2.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); + configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); + configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("ESTIMATOR_STATUS", 1.0f); + configure_stream_local("EXTENDED_SYS_STATE", 1.0f); + configure_stream_local("GLOBAL_POSITION_INT", 5.0f); + configure_stream_local("GPS2_RAW", 1.0f); + configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); + configure_stream_local("GPS_RAW_INT", 1.0f); + configure_stream_local("HOME_POSITION", 0.5f); + configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f); + configure_stream_local("OPTICAL_FLOW_RAD", 1.0f); + configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); + configure_stream_local("PING", 0.1f); + configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f); + configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); + configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); + configure_stream_local("SYS_STATUS", 5.0f); + configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); + configure_stream_local("VFR_HUD", 4.0f); + configure_stream_local("VIBRATION", 0.5f); + configure_stream_local("WIND_COV", 1.0f); + +#if !defined(CONSTRAINED_FLASH) + configure_stream_local("DEBUG", 1.0f); + configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f); + configure_stream_local("DEBUG_VECT", 1.0f); + configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); + configure_stream_local("LINK_NODE_STATUS", 1.0f); +#endif // !CONSTRAINED_FLASH + + break; + + case MAVLINK_MODE_OSD: + configure_stream_local("ALTITUDE", 10.0f); + configure_stream_local("ATTITUDE", 25.0f); + configure_stream_local("ATTITUDE_TARGET", 10.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); + configure_stream_local("ESTIMATOR_STATUS", 1.0f); + configure_stream_local("EXTENDED_SYS_STATE", 1.0f); + configure_stream_local("GLOBAL_POSITION_INT", 10.0f); + configure_stream_local("GPS_RAW_INT", 1.0f); + configure_stream_local("HOME_POSITION", 0.5f); + configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); + configure_stream_local("SYS_STATUS", 5.0f); + configure_stream_local("SYSTEM_TIME", 1.0f); + configure_stream_local("VFR_HUD", 25.0f); + configure_stream_local("VIBRATION", 0.5f); + configure_stream_local("WIND_COV", 2.0f); + break; + + case MAVLINK_MODE_MAGIC: + + /* fallthrough */ + case MAVLINK_MODE_CUSTOM: + //stream nothing + break; + + case MAVLINK_MODE_CONFIG: // USB + // Note: streams requiring low latency come first + configure_stream_local("TIMESYNC", 10.0f); + configure_stream_local("CAMERA_TRIGGER", unlimited_rate); + configure_stream_local("LOCAL_POSITION_NED", 30.0f); + configure_stream_local("DISTANCE_SENSOR", 10.0f); + configure_stream_local("MOUNT_ORIENTATION", 10.0f); + configure_stream_local("ODOMETRY", 30.0f); + + configure_stream_local("ACTUATOR_CONTROL_TARGET0", 30.0f); + configure_stream_local("ADSB_VEHICLE", unlimited_rate); + configure_stream_local("ALTITUDE", 10.0f); + configure_stream_local("ATTITUDE", 50.0f); + configure_stream_local("ATTITUDE_QUATERNION", 50.0f); + configure_stream_local("ATTITUDE_TARGET", 8.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); + configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); + configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("ESC_INFO", 10.0f); + configure_stream_local("ESC_STATUS", 10.0f); + configure_stream_local("ESTIMATOR_STATUS", 5.0f); + configure_stream_local("EXTENDED_SYS_STATE", 2.0f); + configure_stream_local("GLOBAL_POSITION_INT", 10.0f); + configure_stream_local("GPS2_RAW", unlimited_rate); + configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); + configure_stream_local("GPS_RAW_INT", unlimited_rate); + configure_stream_local("GPS_STATUS", 1.0f); + configure_stream_local("HIGHRES_IMU", 50.0f); + configure_stream_local("HOME_POSITION", 0.5f); + configure_stream_local("MANUAL_CONTROL", 5.0f); + configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); + configure_stream_local("OPTICAL_FLOW_RAD", 10.0f); + configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); + configure_stream_local("PING", 1.0f); + configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream_local("RAW_RPM", 5.0f); + configure_stream_local("RC_CHANNELS", 10.0f); + configure_stream_local("SCALED_IMU", 25.0f); + configure_stream_local("SCALED_IMU2", 25.0f); + configure_stream_local("SCALED_IMU3", 25.0f); + configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f); + configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f); + configure_stream_local("SYS_STATUS", 1.0f); + configure_stream_local("SYSTEM_TIME", 1.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); + configure_stream_local("VFR_HUD", 20.0f); + configure_stream_local("VIBRATION", 2.5f); + configure_stream_local("WIND_COV", 10.0f); + +#if !defined(CONSTRAINED_FLASH) + configure_stream_local("DEBUG", 50.0f); + configure_stream_local("DEBUG_FLOAT_ARRAY", 50.0f); + configure_stream_local("DEBUG_VECT", 50.0f); + configure_stream_local("NAMED_VALUE_FLOAT", 50.0f); + configure_stream_local("LINK_NODE_STATUS", 1.0f); +#endif // !CONSTRAINED_FLASH + + break; + + case MAVLINK_MODE_IRIDIUM: + configure_stream_local("HIGH_LATENCY2", 0.015f); + break; + + case MAVLINK_MODE_MINIMAL: + configure_stream_local("ALTITUDE", 0.5f); + configure_stream_local("ATTITUDE", 10.0f); + configure_stream_local("EXTENDED_SYS_STATE", 0.1f); + configure_stream_local("GLOBAL_POSITION_INT", 5.0f); + configure_stream_local("GPS_RAW_INT", 0.5f); + configure_stream_local("HOME_POSITION", 0.1f); + configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); + configure_stream_local("RC_CHANNELS", 0.5f); + configure_stream_local("SYS_STATUS", 0.1f); + configure_stream_local("VFR_HUD", 1.0f); + +#if !defined(CONSTRAINED_FLASH) + configure_stream_local("LINK_NODE_STATUS", 1.0f); +#endif // !CONSTRAINED_FLASH + + break; + + case MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH: + // Note: streams requiring low latency come first + configure_stream_local("TIMESYNC", 10.0f); + configure_stream_local("CAMERA_TRIGGER", unlimited_rate); + configure_stream_local("LOCAL_POSITION_NED", 30.0f); + configure_stream_local("ATTITUDE", 20.0f); + configure_stream_local("ALTITUDE", 10.0f); + configure_stream_local("DISTANCE_SENSOR", 10.0f); + configure_stream_local("MOUNT_ORIENTATION", 10.0f); + configure_stream_local("OBSTACLE_DISTANCE", 10.0f); + configure_stream_local("ODOMETRY", 30.0f); + configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); + + configure_stream_local("ADSB_VEHICLE", unlimited_rate); + configure_stream_local("ATTITUDE_TARGET", 2.0f); + configure_stream_local("BATTERY_STATUS", 0.5f); + configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("ESTIMATOR_STATUS", 1.0f); + configure_stream_local("EXTENDED_SYS_STATE", 1.0f); + configure_stream_local("GLOBAL_POSITION_INT", 10.0f); + configure_stream_local("GPS2_RAW", unlimited_rate); + configure_stream_local("GPS_RAW_INT", unlimited_rate); + configure_stream_local("HOME_POSITION", 0.5f); + configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f); + configure_stream_local("OPTICAL_FLOW_RAD", 1.0f); + configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); + configure_stream_local("PING", 0.1f); + configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f); + configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); + configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); + configure_stream_local("SYS_STATUS", 5.0f); + configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); + configure_stream_local("VFR_HUD", 4.0f); + configure_stream_local("VIBRATION", 0.5f); + configure_stream_local("WIND_COV", 1.0f); + +#if !defined(CONSTRAINED_FLASH) + configure_stream_local("DEBUG", 1.0f); + configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f); + configure_stream_local("DEBUG_VECT", 1.0f); + configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); +#endif // !CONSTRAINED_FLASH + break; + + default: + ret = -1; + break; + } + + if (configure_single_stream && !stream_configured && strcmp(configure_single_stream, "HEARTBEAT") != 0) { + // stream was not found, assume it is disabled by default + return configure_stream(configure_single_stream, 0.0f); + } + + return ret; +} + +int +Mavlink::task_main(int argc, char *argv[]) +{ + int ch; + _baudrate = 57600; + _datarate = 0; + _mode = MAVLINK_MODE_COUNT; + FLOW_CONTROL_MODE _flow_control = FLOW_CONTROL_AUTO; + + _interface_name = nullptr; + + // We don't care about the name and verb at this point. + argc -= 2; + argv += 2; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + int myoptind = 1; + const char *myoptarg = nullptr; +#if defined(CONFIG_NET) || defined(__PX4_POSIX) + int temp_int_arg; +#endif + + while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZp", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'b': + if (px4_get_parameter_value(myoptarg, _baudrate) != 0) { + PX4_ERR("baudrate parsing failed"); + err_flag = true; + } + + if (_baudrate < 9600 || _baudrate > 3000000) { + PX4_ERR("invalid baud rate '%s'", myoptarg); + err_flag = true; + } + + break; + + case 'r': + if (px4_get_parameter_value(myoptarg, _datarate) != 0) { + PX4_ERR("datarate parsing failed"); + err_flag = true; + } + + if (_datarate > MAX_DATA_RATE) { + PX4_ERR("invalid data rate '%s'", myoptarg); + err_flag = true; + } + + break; + + case 'd': + _device_name = myoptarg; + set_protocol(Protocol::SERIAL); + + if (access(_device_name, F_OK) == -1) { + PX4_ERR("Device %s does not exist", _device_name); + err_flag = true; + } + + break; + + case 'n': + _interface_name = myoptarg; + break; + +#if defined(MAVLINK_UDP) + + case 'u': + if (px4_get_parameter_value(myoptarg, temp_int_arg) != 0) { + PX4_ERR("invalid data udp_port"); + err_flag = true; + + } else { + _network_port = temp_int_arg; + set_protocol(Protocol::UDP); + } + + break; + + case 'o': + if (px4_get_parameter_value(myoptarg, temp_int_arg) != 0) { + PX4_ERR("invalid remote udp_port"); + err_flag = true; + + } else { + _remote_port = temp_int_arg; + set_protocol(Protocol::UDP); + } + + break; + + case 't': + _src_addr.sin_family = AF_INET; + + if (inet_aton(myoptarg, &_src_addr.sin_addr)) { + _src_addr_initialized = true; + + } else { + PX4_ERR("invalid partner ip '%s'", myoptarg); + err_flag = true; + } + + break; + + case 'p': + _mav_broadcast = BROADCAST_MODE_ON; + break; + +#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) + + // multicast + case 'c': + _src_addr.sin_family = AF_INET; + + if (inet_aton(myoptarg, &_src_addr.sin_addr)) { + _src_addr_initialized = true; + _mav_broadcast = BROADCAST_MODE_MULTICAST; + + } else { + PX4_ERR("invalid partner ip '%s'", myoptarg); + err_flag = true; + } + + break; +#else + + case 'c': + PX4_ERR("Multicast option is not supported on this platform"); + err_flag = true; + break; +#endif +#else + + case 'p': + case 'u': + case 'o': + case 't': + PX4_ERR("UDP options not supported on this platform"); + err_flag = true; + break; +#endif + +// case 'e': +// _mavlink_link_termination_allowed = true; +// break; + + case 'm': { + + int mode; + + if (px4_get_parameter_value(myoptarg, mode) == 0) { + if (mode >= 0 && mode < (int)MAVLINK_MODE_COUNT) { + _mode = (MAVLINK_MODE)mode; + + } else { + PX4_ERR("invalid mode"); + err_flag = true; + } + + } else { + if (strcmp(myoptarg, "custom") == 0) { + _mode = MAVLINK_MODE_CUSTOM; + + } else if (strcmp(myoptarg, "camera") == 0) { + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + + } else if (strcmp(myoptarg, "onboard") == 0) { + _mode = MAVLINK_MODE_ONBOARD; + + } else if (strcmp(myoptarg, "osd") == 0) { + _mode = MAVLINK_MODE_OSD; + + } else if (strcmp(myoptarg, "magic") == 0) { + _mode = MAVLINK_MODE_MAGIC; + + } else if (strcmp(myoptarg, "config") == 0) { + _mode = MAVLINK_MODE_CONFIG; + + } else if (strcmp(myoptarg, "iridium") == 0) { + _mode = MAVLINK_MODE_IRIDIUM; + set_telemetry_status_type(telemetry_status_s::LINK_TYPE_IRIDIUM); + + } else if (strcmp(myoptarg, "minimal") == 0) { + _mode = MAVLINK_MODE_MINIMAL; + + } else if (strcmp(myoptarg, "extvision") == 0) { + _mode = MAVLINK_MODE_EXTVISION; + + } else if (strcmp(myoptarg, "extvisionmin") == 0) { + _mode = MAVLINK_MODE_EXTVISIONMIN; + + } else if (strcmp(myoptarg, "gimbal") == 0) { + _mode = MAVLINK_MODE_GIMBAL; + + } else if (strcmp(myoptarg, "onboard_low_bandwidth") == 0) { + _mode = MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH; + + } else { + PX4_ERR("invalid mode"); + err_flag = true; + } + } + + break; + } + + case 'f': + _forwarding_on = true; + break; + + case 's': + _use_software_mav_throttling = true; + break; + + case 'w': + _wait_to_transmit = true; + break; + + case 'x': + _ftp_on = true; + break; + + case 'z': + _flow_control = FLOW_CONTROL_ON; + break; + + case 'Z': + _flow_control = FLOW_CONTROL_OFF; + break; + + default: + err_flag = true; + break; + } + } + + if (err_flag) { + usage(); + return PX4_ERROR; + } + + /* USB serial is indicated by /dev/ttyACMx */ + if (strcmp(_device_name, "/dev/ttyACM0") == OK || strcmp(_device_name, "/dev/ttyACM1") == OK) { + if (_datarate == 0) { + _datarate = 800000; + } + + if (_mode == MAVLINK_MODE_COUNT) { + _mode = MAVLINK_MODE_CONFIG; + } + + _ftp_on = true; + _is_usb_uart = true; + } + + if (_mode == MAVLINK_MODE_COUNT) { + _mode = MAVLINK_MODE_NORMAL; + } + + if (_datarate == 0) { + /* convert bits to bytes and use 1/2 of bandwidth by default */ + _datarate = _baudrate / 20; + } + + if (_datarate > MAX_DATA_RATE) { + _datarate = MAX_DATA_RATE; + } + + if (get_protocol() == Protocol::SERIAL) { + if (Mavlink::serial_instance_exists(_device_name, this)) { + PX4_ERR("%s already running", _device_name); + return PX4_ERROR; + } + + PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB", + mavlink_mode_str(_mode), _datarate, _device_name, _baudrate); + + /* flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) { + PX4_ERR("port %hu already occupied", _network_port); + return PX4_ERROR; + } + + PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu", + mavlink_mode_str(_mode), _datarate, _network_port, _remote_port); + } + +#endif // MAVLINK_UDP + + if (!set_instance_id()) { + PX4_ERR("no instances available"); + return PX4_ERROR; + } + + set_channel(); + + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, nullptr); + + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_forwarding_on) { + /* initialize message buffer if multiplexing is on. + * make space for two messages plus off-by-one space as we use the empty element + * marker ring buffer approach. + */ + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + PX4_ERR("msg buf alloc fail"); + return 1; + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, nullptr); + } + + /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ + _transmitting_enabled = true; + _transmitting_enabled_commanded = true; + + if (_mode == MAVLINK_MODE_IRIDIUM) { + _transmitting_enabled_commanded = false; + } + + /* add default streams depending on mode */ + if (_mode != MAVLINK_MODE_IRIDIUM) { + + /* HEARTBEAT is constant rate stream, rate never adjusted */ + configure_stream("HEARTBEAT", 1.0f); + + /* STATUSTEXT stream */ + configure_stream("STATUSTEXT", 20.0f); + + /* COMMAND_LONG stream: use unlimited rate to send all commands */ + configure_stream("COMMAND_LONG"); + + } + + if (configure_streams_to_default() != 0) { + PX4_ERR("configure_streams_to_default() failed"); + } + + /* set main loop delay depending on data rate to minimize CPU overhead */ + _main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate; + + /* hard limit to 1000 Hz at max */ + if (_main_loop_delay < MAVLINK_MIN_INTERVAL) { + _main_loop_delay = MAVLINK_MIN_INTERVAL; + } + + /* hard limit to 100 Hz at least */ + if (_main_loop_delay > MAVLINK_MAX_INTERVAL) { + _main_loop_delay = MAVLINK_MAX_INTERVAL; + } + + /* open the UART device after setting the instance, as it might block */ + if (get_protocol() == Protocol::SERIAL) { + _uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control); + + if (_uart_fd < 0 && !_is_usb_uart) { + PX4_ERR("could not open %s", _device_name); + return PX4_ERROR; + + } else if (_uart_fd < 0 && _is_usb_uart) { + /* the config link is optional */ + return PX4_OK; + } + } + +#if defined(MAVLINK_UDP) + + /* init socket if necessary */ + if (get_protocol() == Protocol::UDP) { + init_udp(); + } + +#endif // MAVLINK_UDP + + /* if the protocol is serial, we send the system version blindly */ + if (get_protocol() == Protocol::SERIAL) { + send_autopilot_capabilities(); + } + + _receiver.start(); + + _mavlink_start_time = hrt_absolute_time(); + + while (!_task_should_exit) { + /* main loop */ + px4_usleep(_main_loop_delay); + + if (!should_transmit()) { + check_requested_subscriptions(); + continue; + } + + perf_count(_loop_interval_perf); + perf_begin(_loop_perf); + + const hrt_abstime t = hrt_absolute_time(); + + update_rate_mult(); + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + mavlink_update_parameters(); + +#if defined(CONFIG_NET) + + if (!multicast_enabled()) { + _src_addr_initialized = false; + } + +#endif // CONFIG_NET + } + + configure_sik_radio(); + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + /* switch HIL mode if required */ + set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON); + + set_generate_virtual_rc_input(vehicle_status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); + + if (_mode == MAVLINK_MODE_IRIDIUM) { + + if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost && + !_transmitting_enabled_commanded && _first_heartbeat_sent) { + + _transmitting_enabled = false; + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name); + + } else if (!_transmitting_enabled && !vehicle_status.high_latency_data_link_lost) { + _transmitting_enabled = true; + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name); + } + } + } + } + + + // vehicle_command + if (_mode == MAVLINK_MODE_IRIDIUM) { + while (_vehicle_command_sub.updated()) { + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); + vehicle_command_s vehicle_cmd; + + if (_vehicle_command_sub.update(&vehicle_cmd)) { + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation()); + } + + if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && + _mode == MAVLINK_MODE_IRIDIUM) { + + if (vehicle_cmd.param1 > 0.5f) { + if (!_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command", + _device_name); + } + + _transmitting_enabled = true; + _transmitting_enabled_commanded = true; + + } else { + if (_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command", + _device_name); + } + + _transmitting_enabled = false; + _transmitting_enabled_commanded = false; + } + + // send positive command ack + vehicle_command_ack_s command_ack{}; + command_ack.command = vehicle_cmd.command; + command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + command_ack.from_external = !vehicle_cmd.from_external; + command_ack.target_system = vehicle_cmd.source_system; + command_ack.target_component = vehicle_cmd.source_component; + command_ack.timestamp = vehicle_cmd.timestamp; + _vehicle_command_ack_pub.publish(command_ack); + } + } + } + } + + /* send command ACK */ + bool cmd_logging_start_acknowledgement = false; + + if (_vehicle_command_ack_sub.updated()) { + static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.updated()) { + vehicle_command_ack_s command_ack; + const unsigned last_generation = _vehicle_command_ack_sub.get_last_generation(); + + if (_vehicle_command_ack_sub.update(&command_ack)) { + if (_vehicle_command_ack_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command_ack lost, generation %u -> %u", last_generation, + _vehicle_command_ack_sub.get_last_generation()); + } + + if (!command_ack.from_external && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { + mavlink_command_ack_t msg{}; + msg.result = command_ack.result; + msg.command = command_ack.command; + msg.progress = command_ack.result_param1; + msg.result_param2 = command_ack.result_param2; + msg.target_system = command_ack.target_system; + msg.target_component = command_ack.target_component; + + // TODO: always transmit the acknowledge once it is only sent over the instance the command is received + //bool _transmitting_enabled_temp = _transmitting_enabled; + //_transmitting_enabled = true; + mavlink_msg_command_ack_send_struct(get_channel(), &msg); + //_transmitting_enabled = _transmitting_enabled_temp; + + if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + cmd_logging_start_acknowledgement = true; + } + } + } + } + } + + /* check for shell output */ + if (_mavlink_shell && _mavlink_shell->available() > 0) { + if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) { + mavlink_serial_control_t msg; + msg.baudrate = 0; + msg.flags = SERIAL_CONTROL_FLAG_REPLY; + msg.timeout = 0; + msg.device = SERIAL_CONTROL_DEV_SHELL; + msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data)); + mavlink_msg_serial_control_send_struct(get_channel(), &msg); + } + } + + check_requested_subscriptions(); + + /* update streams */ + for (const auto &stream : _streams) { + stream->update(t); + + if (!_first_heartbeat_sent) { + if (_mode == MAVLINK_MODE_IRIDIUM) { + if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) { + _first_heartbeat_sent = stream->first_message_sent(); + } + + } else { + if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) { + _first_heartbeat_sent = stream->first_message_sent(); + } + } + } + } + + /* check for ulog streaming messages */ + if (_mavlink_ulog) { + if (_mavlink_ulog_stop_requested.load()) { + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; + _mavlink_ulog_stop_requested.store(false); + + } else { + if (cmd_logging_start_acknowledgement) { + _mavlink_ulog->start_ack_received(); + } + + int ret = _mavlink_ulog->handle_update(get_channel()); + + if (ret < 0) { //abort the streaming on error + if (ret != -1) { + PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret); + } + + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; + } + } + } + + /* pass messages from other UARTs */ + if (_forwarding_on) { + + bool is_part; + uint8_t *read_ptr; + uint8_t *write_ptr; + + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); + + /* write second part of buffer if there is some */ + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); + message_buffer_mark_read(available); + } + + pthread_mutex_unlock(&_message_buffer_mutex); + + resend_message(&msg); + } + } + + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1_s) { + if (_bytes_timestamp != 0) { + const float dt = (t - _bytes_timestamp) * 1e-6f; + + _tstatus.tx_rate_avg = _bytes_tx / dt; + _tstatus.tx_error_rate_avg = _bytes_txerr / dt; + _tstatus.rx_rate_avg = _bytes_rx / dt; + + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + + _bytes_timestamp = t; + } + + // publish status at 1 Hz, or sooner if HEARTBEAT has updated + if ((hrt_elapsed_time(&_tstatus.timestamp) >= 1_s) || _tstatus_updated) { + publish_telemetry_status(); + } + + perf_end(_loop_perf); + } + + _receiver.stop(); + + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + + /* delete streams */ + _streams.clear(); + + if (_uart_fd >= 0 && !_is_usb_uart) { + /* discard all pending data, as close() might block otherwise on NuttX with flow control enabled */ + tcflush(_uart_fd, TCIOFLUSH); + /* close UART */ + ::close(_uart_fd); + } + + if (_socket_fd >= 0) { + close(_socket_fd); + _socket_fd = -1; + } + + if (_forwarding_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } + + if (_mavlink_ulog) { + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; + } + + pthread_mutex_destroy(&_send_mutex); + + _task_running = false; + + PX4_INFO("exiting channel %i", (int)_channel); + + return OK; +} + +void Mavlink::check_requested_subscriptions() +{ + if (_subscribe_to_stream != nullptr) { + if (_subscribe_to_stream_rate < -1.5f) { + if (configure_streams_to_default(_subscribe_to_stream) == 0) { + if (get_protocol() == Protocol::SERIAL) { + PX4_DEBUG("stream %s on device %s set to default rate", _subscribe_to_stream, _device_name); + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + PX4_DEBUG("stream %s on UDP port %hu set to default rate", _subscribe_to_stream, _network_port); + } + +#endif // MAVLINK_UDP + + } else { + PX4_ERR("setting stream %s to default failed", _subscribe_to_stream); + } + + } else if (configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate) == 0) { + if (fabsf(_subscribe_to_stream_rate) > 0.00001f) { + if (get_protocol() == Protocol::SERIAL) { + PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); + + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + PX4_DEBUG("stream %s on UDP port %hu enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, + (double)_subscribe_to_stream_rate); + } + +#endif // MAVLINK_UDP + + } else { + if (get_protocol() == Protocol::SERIAL) { + PX4_DEBUG("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + PX4_DEBUG("stream %s on UDP port %hu disabled", _subscribe_to_stream, _network_port); + } + +#endif // MAVLINK_UDP + } + + } else { + if (get_protocol() == Protocol::SERIAL) { + PX4_ERR("stream %s on device %s not found", _subscribe_to_stream, _device_name); + + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + PX4_ERR("stream %s on UDP port %hu not found", _subscribe_to_stream, _network_port); + } + +#endif // MAVLINK_UDP + } + + _subscribe_to_stream = nullptr; + } +} + +void Mavlink::publish_telemetry_status() +{ + // many fields are populated in place + + _tstatus.mode = _mode; + _tstatus.data_rate = _datarate; + _tstatus.rate_multiplier = _rate_mult; + _tstatus.flow_control = get_flow_control_enabled(); + _tstatus.ftp = ftp_enabled(); + _tstatus.forwarding = get_forwarding_on(); + _tstatus.mavlink_v2 = (_protocol_version == 2); + + _tstatus.streams = _streams.size(); + + // telemetry_status is also updated from the receiver thread, but never the same fields + _tstatus.timestamp = hrt_absolute_time(); + _telemetry_status_pub.publish(_tstatus); + _tstatus_updated = false; +} + +void Mavlink::configure_sik_radio() +{ + /* radio config check */ + if (_uart_fd >= 0 && _param_sik_radio_id.get() != 0) { + /* request to configure radio and radio is present */ + FILE *fs = fdopen(_uart_fd, "w"); + + if (fs) { + /* switch to AT command mode */ + px4_usleep(1200000); + fprintf(fs, "+++\n"); + px4_usleep(1200000); + + if (_param_sik_radio_id.get() > 0) { + /* set channel */ + fprintf(fs, "ATS3=%" PRIu32 "\n", _param_sik_radio_id.get()); + px4_usleep(200000); + + } else { + /* reset to factory defaults */ + fprintf(fs, "AT&F\n"); + px4_usleep(200000); + } + + /* write config */ + fprintf(fs, "AT&W"); + px4_usleep(200000); + + /* reboot */ + fprintf(fs, "ATZ"); + px4_usleep(200000); + + // XXX NuttX suffers from a bug where + // fclose() also closes the fd, not just + // the file stream. Since this is a one-time + // config thing, we leave the file struct + // allocated. +#ifndef __PX4_NUTTX + fclose(fs); +#endif + + } else { + PX4_WARN("open fd %d failed", _uart_fd); + } + + /* reset param and save */ + _param_sik_radio_id.set(0); + _param_sik_radio_id.commit_no_notification(); + } +} + +int Mavlink::start_helper(int argc, char *argv[]) +{ + /* create the instance in task context */ + Mavlink *instance = new Mavlink(); + + int res; + + if (!instance) { + + /* out of memory */ + res = -ENOMEM; + PX4_ERR("OUT OF MEM"); + + } else { + /* this will actually only return once MAVLink exits */ + res = instance->task_main(argc, argv); + instance->_task_running = false; + + } + + return res; +} + +int +Mavlink::start(int argc, char *argv[]) +{ + MavlinkULog::initialize(); + MavlinkCommandSender::initialize(); + + // Wait for the instance count to go up one + // before returning to the shell + int ic = Mavlink::instance_count(); + + if (ic == MAVLINK_COMM_NUM_BUFFERS) { + PX4_ERR("Maximum MAVLink instance count of %d reached.", MAVLINK_COMM_NUM_BUFFERS); + return 1; + } + + // Instantiate thread + char buf[24]; + sprintf(buf, "mavlink_if%d", ic); + + // This is where the control flow splits + // between the starting task and the spawned + // task - start_helper() only returns + // when the started task exits. + px4_task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2896 + MAVLINK_NET_ADDED_STACK, + (px4_main_t)&Mavlink::start_helper, + (char *const *)argv); + + // Ensure that this shell command + // does not return before the instance + // is fully initialized. As this is also + // the only path to create a new instance, + // this is effectively a lock on concurrent + // instance starting. XXX do a real lock. + + // Sleep 500 us between each attempt + const unsigned sleeptime = 500; + + // Wait 100 ms max for the startup. + const unsigned limit = 100 * 1000 / sleeptime; + + unsigned count = 0; + + while (ic == Mavlink::instance_count() && count < limit) { + px4_usleep(sleeptime); + count++; + } + + if (ic == Mavlink::instance_count()) { + return PX4_ERROR; + + } else { + return PX4_OK; + } +} + +void +Mavlink::display_status() +{ + if (_tstatus.heartbeat_type_gcs) { + printf("\tGCS heartbeat valid\n"); + } + + printf("\tmavlink chan: #%u\n", static_cast(_channel)); + + if (_tstatus.timestamp > 0) { + + printf("\ttype:\t\t"); + + if (_radio_status_available) { + printf("RADIO Link\n"); + printf("\t rssi:\t\t%" PRIu8 "\n", _rstatus.rssi); + printf("\t remote rssi:\t%" PRIu8 "\n", _rstatus.remote_rssi); + printf("\t txbuf:\t%" PRIu8 "\n", _rstatus.txbuf); + printf("\t noise:\t%" PRIu8 "\n", _rstatus.noise); + printf("\t remote noise:\t%" PRIu8 "\n", _rstatus.remote_noise); + printf("\t rx errors:\t%" PRIu16 "\n", _rstatus.rxerrors); + printf("\t fixed:\t%" PRIu16 "\n", _rstatus.fix); + + } else if (_is_usb_uart) { + printf("USB CDC\n"); + + } else { + printf("GENERIC LINK OR RADIO\n"); + } + + } else { + printf("\tno radio status.\n"); + } + + printf("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF"); + printf("\trates:\n"); + printf("\t tx: %.1f B/s\n", (double)_tstatus.tx_rate_avg); + printf("\t txerr: %.1f B/s\n", (double)_tstatus.tx_error_rate_avg); + printf("\t tx rate mult: %.3f\n", (double)_rate_mult); + printf("\t tx rate max: %i B/s\n", _datarate); + printf("\t rx: %.1f B/s\n", (double)_tstatus.rx_rate_avg); + printf("\t rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate); + _receiver.print_detailed_rx_stats(); + + if (_mavlink_ulog) { + printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100., + (double)_mavlink_ulog->maximum_data_rate() * 100.); + } + + printf("\tFTP enabled: %s, TX enabled: %s\n", + _ftp_on ? "YES" : "NO", + _transmitting_enabled ? "YES" : "NO"); + printf("\tmode: %s\n", mavlink_mode_str(_mode)); + printf("\tMAVLink version: %" PRId32 "\n", _protocol_version); + + printf("\ttransport protocol: "); + + switch (_protocol) { +#if defined(MAVLINK_UDP) + + case Protocol::UDP: + printf("UDP (%hu, remote port: %hu)\n", _network_port, _remote_port); + printf("\tBroadcast enabled: %s\n", + broadcast_enabled() ? "YES" : "NO"); +#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) + printf("\tMulticast enabled: %s\n", + multicast_enabled() ? "YES" : "NO"); +#endif +#ifdef __PX4_POSIX + + if (get_client_source_initialized()) { + printf("\tpartner IP: %s\n", inet_ntoa(get_client_source_address().sin_addr)); + } + +#endif + break; +#endif // MAVLINK_UDP + + case Protocol::SERIAL: + printf("serial (%s @%i)\n", _device_name, _baudrate); + break; + } + + if (_ping_stats.last_ping_time > 0) { + printf("\tping statistics:\n"); + printf("\t last: %0.2f ms\n", (double)_ping_stats.last_rtt); + printf("\t mean: %0.2f ms\n", (double)_ping_stats.mean_rtt); + printf("\t max: %0.2f ms\n", (double)_ping_stats.max_rtt); + printf("\t min: %0.2f ms\n", (double)_ping_stats.min_rtt); + printf("\t dropped packets: %" PRIi32 "\n", _ping_stats.dropped_packets); + } +} + +void +Mavlink::display_status_streams() +{ + printf("\t%-20s%-16s %s\n", "Name", "Rate Config (current) [Hz]", "Message Size (if active) [B]"); + + const float rate_mult = _rate_mult; + + for (const auto &stream : _streams) { + const int interval = stream->get_interval(); + const unsigned size = stream->get_size(); + char rate_str[20]; + + if (interval < 0) { + strcpy(rate_str, "unlimited"); + + } else { + float rate = 1000000.0f / (float)interval; + // Note that the actual current rate can be lower if the associated uORB topic updates at a + // lower rate. + float rate_current = stream->const_rate() ? rate : rate * rate_mult; + snprintf(rate_str, sizeof(rate_str), "%6.2f (%.3f)", (double)rate, (double)rate_current); + } + + printf("\t%-30s%-16s", stream->get_name(), rate_str); + + if (size > 0) { + printf(" %3u\n", size); + + } else { + printf("\n"); + } + } +} + +int +Mavlink::stream_command(int argc, char *argv[]) +{ + const char *device_name = DEFAULT_DEVICE_NAME; + float rate = -1.0f; + const char *stream_name = nullptr; +#ifdef MAVLINK_UDP + int temp_int_arg; + unsigned short network_port = 0; +#endif // MAVLINK_UDP + bool provided_device = false; + bool provided_network_port = false; + + /* + * Called via main with original argv + * mavlink start + * + * Remove 2 + */ + argc -= 2; + argv += 2; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + + int i = 0; + + while (i < argc) { + + if (0 == strcmp(argv[i], "-r") && i < argc - 1) { + rate = strtod(argv[i + 1], nullptr); + + if (rate < 0.0f) { + err_flag = true; + } + + i++; + + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + provided_device = true; + device_name = argv[i + 1]; + i++; + + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { + stream_name = argv[i + 1]; + i++; + +#ifdef MAVLINK_UDP + + } else if (0 == strcmp(argv[i], "-u") && i < argc - 1) { + provided_network_port = true; + + if (px4_get_parameter_value(argv[i + 1], temp_int_arg) != 0) { + err_flag = true; + + } else { + network_port = temp_int_arg; + } + + i++; +#endif // MAVLINK_UDP + + } else { + err_flag = true; + } + + i++; + } + + if (!err_flag && stream_name != nullptr) { + + Mavlink *inst = nullptr; + + if (provided_device && !provided_network_port) { + inst = get_instance_for_device(device_name); + +#ifdef MAVLINK_UDP + + } else if (provided_network_port && !provided_device) { + inst = get_instance_for_network_port(network_port); +#endif // MAVLINK_UDP + + } else if (provided_device && provided_network_port) { + PX4_WARN("please provide either a device name or a network port"); + return 1; + } + + if (rate < 0.0f) { + rate = -2.0f; // use default rate + } + + if (inst != nullptr) { + inst->configure_stream_threadsafe(stream_name, rate); + + } else { + + // If the link is not running we should complain, but not fall over + // because this is so easy to get wrong and not fatal. Warning is sufficient. + if (provided_device) { + PX4_WARN("mavlink for device %s is not running", device_name); + + } + +#ifdef MAVLINK_UDP + + else { + PX4_WARN("mavlink for network on port %hu is not running", network_port); + } + +#endif // MAVLINK_UDP + + return 1; + } + + } else { + usage(); + return 1; + } + + return OK; +} + +void +Mavlink::set_boot_complete() +{ + _boot_complete = true; + +#if defined(MAVLINK_UDP) + LockGuard lg {mavlink_module_mutex}; + + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst->get_mode() != MAVLINK_MODE_ONBOARD) && + !inst->broadcast_enabled() && inst->get_protocol() == Protocol::UDP) { + + PX4_INFO("MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)"); + } + } + +#endif // MAVLINK_UDP + +} + +static void usage() +{ + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module implements the MAVLink protocol, which can be used on a Serial link or UDP network connection. +It communicates with the system via uORB: some messages are directly handled in the module (eg. mission +protocol), others are published via uORB (eg. vehicle_command). + +Streams are used to send periodic messages with a specific rate, such as the vehicle attitude. +When starting the mavlink instance, a mode can be specified, which defines the set of enabled streams with their rates. +For a running instance, streams can be configured via `mavlink stream` command. + +There can be multiple independent instances of the module, each connected to one serial device or network port. + +### Implementation +The implementation uses 2 threads, a sending and a receiving thread. The sender runs at a fixed rate and dynamically +reduces the rates of the streams if the combined bandwidth is higher than the configured rate (`-r`) or the +physical link becomes saturated. This can be checked with `mavlink status`, see if `rate mult` is less than 1. + +**Careful**: some of the data is accessed and modified from both threads, so when changing code or extend the +functionality, this needs to be take into account, in order to avoid race conditions and corrupt data. + +### Examples +Start mavlink on ttyS1 serial with baudrate 921600 and maximum sending rate of 80kB/s: +$ mavlink start -d /dev/ttyS1 -b 921600 -m onboard -r 80000 + +Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz: +$ mavlink start -u 14556 -r 1000000 +$ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mavlink", "communication"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start a new instance"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS1", "", "Select Serial Device", true); + PRINT_MODULE_USAGE_PARAM_INT('b', 57600, 9600, 3000000, "Baudrate (can also be p:)", true); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 10, 10000000, "Maximum sending data rate in B/s (if 0, use baudrate / 20)", true); +#if defined(CONFIG_NET) || defined(__PX4_POSIX) + PRINT_MODULE_USAGE_PARAM_FLAG('p', "Enable Broadcast", true); + PRINT_MODULE_USAGE_PARAM_INT('u', 14556, 0, 65536, "Select UDP Network Port (local)", true); + PRINT_MODULE_USAGE_PARAM_INT('o', 14550, 0, 65536, "Select UDP Network Port (remote)", true); + PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, "Partner IP (broadcasting can be enabled via -p flag)", true); +#endif + PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal", + "Mode: sets default streams and rates", true); + PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "", "wifi/ethernet interface name", true); +#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) + PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, "Multicast address in the range [239.0.0.0,239.255.255.255]", "Multicast address (multicasting can be enabled via MAV_{i}_BROADCAST param)", true); +#endif + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true); + PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true); + PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable FTP", true); + PRINT_MODULE_USAGE_PARAM_FLAG('z', "Force hardware flow control always on", true); + PRINT_MODULE_USAGE_PARAM_FLAG('Z', "Force hardware flow control always off", true); + + PRINT_MODULE_USAGE_COMMAND_DESCR("stop-all", "Stop all instances"); + + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status for all instances"); + PRINT_MODULE_USAGE_ARG("streams", "Print all enabled streams", true); + + PRINT_MODULE_USAGE_COMMAND_DESCR("stream", "Configure the sending rate of a stream for a running instance"); +#if defined(CONFIG_NET) || defined(__PX4_POSIX) + PRINT_MODULE_USAGE_PARAM_INT('u', -1, 0, 65536, "Select Mavlink instance via local Network Port", true); +#endif + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Select Mavlink instance via Serial Device", true); + PRINT_MODULE_USAGE_PARAM_STRING('s', nullptr, nullptr, "Mavlink stream to configure", false); + PRINT_MODULE_USAGE_PARAM_FLOAT('r', -1.0f, 0.0f, 2000.0f, "Rate in Hz (0 = turn off, -1 = set to default)", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("boot_complete", + "Enable sending of messages. (Must be) called as last step in startup script."); + +} + +extern "C" __EXPORT int mavlink_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + return Mavlink::start(argc, argv); + + } else if (!strcmp(argv[1], "stop")) { + PX4_WARN("mavlink stop is deprecated, use stop-all instead"); + usage(); + return 1; + + } else if (!strcmp(argv[1], "stop-all")) { + return Mavlink::destroy_all_instances(); + + } else if (!strcmp(argv[1], "status")) { + bool show_streams_status = argc > 2 && strcmp(argv[2], "streams") == 0; + return Mavlink::get_status_all_instances(show_streams_status); + + } else if (!strcmp(argv[1], "stream")) { + return Mavlink::stream_command(argc, argv); + + } else if (!strcmp(argv[1], "boot_complete")) { + Mavlink::set_boot_complete(); + return 0; + + } else { + usage(); + return 1; + } + + return 0; +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_main.h b/src/prometheus_px4/src/modules/mavlink/mavlink_main.h new file mode 100644 index 0000000..2cc5961 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_main.h @@ -0,0 +1,760 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink_main.h + * + * MAVLink 2.0 protocol interface definition. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +#pragma once + +#include +#include + +#ifdef __PX4_NUTTX +#include +#else +#include +#include +#include +#endif + +#if defined(CONFIG_NET) || !defined(__PX4_NUTTX) +#include +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mavlink_command_sender.h" +#include "mavlink_messages.h" +#include "mavlink_receiver.h" +#include "mavlink_shell.h" +#include "mavlink_ulog.h" + +#define DEFAULT_BAUD_RATE 57600 +#define DEFAULT_DEVICE_NAME "/dev/ttyS1" + +#define HASH_PARAM "_HASH_CHECK" + +#if defined(CONFIG_NET) || defined(__PX4_POSIX) +# define MAVLINK_UDP +# define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec +#endif // CONFIG_NET || __PX4_POSIX + +enum class Protocol { + SERIAL = 0, +#if defined(MAVLINK_UDP) + UDP, +#endif // MAVLINK_UDP +}; + +using namespace time_literals; + +class Mavlink final : public ModuleParams +{ + +public: + /** + * Constructor + */ + Mavlink(); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + static int start(int argc, char *argv[]); + + /** + * Display the mavlink status. + */ + void display_status(); + + /** + * Display the status of all enabled streams. + */ + void display_status_streams(); + + static int stream_command(int argc, char *argv[]); + + static int instance_count(); + + static Mavlink *new_instance(); + + static Mavlink *get_instance_for_device(const char *device_name); + + mavlink_message_t *get_buffer() { return &_mavlink_buffer; } + + mavlink_status_t *get_status() { return &_mavlink_status; } + + /** + * Set the MAVLink version + * + * Currently supporting v1 and v2 + * + * @param version MAVLink version + */ + void set_proto_version(unsigned version); + + static int destroy_all_instances(); + + static int get_status_all_instances(bool show_streams_status); + + static bool serial_instance_exists(const char *device_name, Mavlink *self); + + static void forward_message(const mavlink_message_t *msg, Mavlink *self); + + int get_uart_fd() const { return _uart_fd; } + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id() const { return mavlink_system.sysid; } + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id() const { return mavlink_system.compid; } + + const char *_device_name{DEFAULT_DEVICE_NAME}; + + enum MAVLINK_MODE { + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_ONBOARD, + MAVLINK_MODE_OSD, + MAVLINK_MODE_MAGIC, + MAVLINK_MODE_CONFIG, + MAVLINK_MODE_IRIDIUM, + MAVLINK_MODE_MINIMAL, + MAVLINK_MODE_EXTVISION, + MAVLINK_MODE_EXTVISIONMIN, + MAVLINK_MODE_GIMBAL, + MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH, + MAVLINK_MODE_COUNT + }; + + enum BROADCAST_MODE { + BROADCAST_MODE_OFF = 0, + BROADCAST_MODE_ON, + BROADCAST_MODE_MULTICAST + }; + + enum FLOW_CONTROL_MODE { + FLOW_CONTROL_OFF = 0, + FLOW_CONTROL_AUTO, + FLOW_CONTROL_ON + }; + + static const char *mavlink_mode_str(enum MAVLINK_MODE mode) + { + switch (mode) { + case MAVLINK_MODE_NORMAL: + return "Normal"; + + case MAVLINK_MODE_CUSTOM: + return "Custom"; + + case MAVLINK_MODE_ONBOARD: + return "Onboard"; + + case MAVLINK_MODE_OSD: + return "OSD"; + + case MAVLINK_MODE_MAGIC: + return "Magic"; + + case MAVLINK_MODE_CONFIG: + return "Config"; + + case MAVLINK_MODE_IRIDIUM: + return "Iridium"; + + case MAVLINK_MODE_MINIMAL: + return "Minimal"; + + case MAVLINK_MODE_EXTVISION: + return "ExtVision"; + + case MAVLINK_MODE_EXTVISIONMIN: + return "ExtVisionMin"; + + case MAVLINK_MODE_GIMBAL: + return "Gimbal"; + + case MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH: + return "OnboardLowBandwidth"; + + default: + return "Unknown"; + } + } + + enum MAVLINK_MODE get_mode() { return _mode; } + + bool get_hil_enabled() { return _hil_enabled; } + + bool get_use_hil_gps() { return _param_mav_usehilgps.get(); } + + bool get_forward_externalsp() { return _param_mav_fwdextsp.get(); } + + bool get_flow_control_enabled() { return _flow_control_mode; } + + bool get_forwarding_on() { return _forwarding_on; } + + bool is_connected() { return _tstatus.heartbeat_type_gcs; } + +#if defined(MAVLINK_UDP) + static Mavlink *get_instance_for_network_port(unsigned long port); + + bool broadcast_enabled() { return _mav_broadcast == BROADCAST_MODE_ON; } + + bool multicast_enabled() { return _mav_broadcast == BROADCAST_MODE_MULTICAST; } +#endif // MAVLINK_UDP + + /** + * Set the boot complete flag on all instances + * + * Setting the flag unblocks parameter transmissions, which are gated + * beforehand to ensure that the system is fully initialized. + */ + static void set_boot_complete(); + + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + + static int start_helper(int argc, char *argv[]); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_enabled(bool hil_enabled); + + /** + * Set manual input generation mode + * + * Set to true to generate RC_INPUT messages on the system bus from + * MAVLink messages. + * + * @param generation_enabled If set to true, generate RC_INPUT messages + */ + void set_generate_virtual_rc_input(bool generation_enabled) { _generate_rc = generation_enabled; } + + /** + * Set communication protocol for this mavlink instance + */ + void set_protocol(Protocol p) { _protocol = p; } + + /** + * Get the manual input generation mode + * + * @return true if manual inputs should generate RC data + */ + bool should_generate_virtual_rc_input() { return _generate_rc; } + + /** + * This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction + */ + void send_start(int length); + + /** + * Buffer bytes to send out on the link. + */ + void send_bytes(const uint8_t *buf, unsigned packet_len); + + /** + * Flush the transmit buffer and send one MAVLink packet + */ + void send_finish(); + + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg) { _mavlink_resend_uart(_channel, msg); } + + void handle_message(const mavlink_message_t *msg); + + int get_instance_id() const { return _instance_id; } + + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int setup_flow_control(enum FLOW_CONTROL_MODE enabled); + + mavlink_channel_t get_channel() const { return _channel; } + + void configure_stream_threadsafe(const char *stream_name, float rate = -1.0f); + + orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } + + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level + */ + void send_statustext(unsigned char severity, const char *string); + + /** + * Send the capabilities of this autopilot in terms of the MAVLink spec + */ + bool send_autopilot_capabilities(); + + /** + * Send the protocol version of MAVLink + */ + void send_protocol_version(); + + List &get_streams() { return _streams; } + + float get_rate_mult() const { return _rate_mult; } + + float get_baudrate() { return _baudrate; } + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (_transmitting_enabled && _boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); } + + bool message_buffer_write(const void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + + /** + * Get the receive status of this MAVLink link + */ + telemetry_status_s &telemetry_status() { return _tstatus; } + void telemetry_status_updated() { _tstatus_updated = true; } + + void set_telemetry_status_type(uint8_t type) { _tstatus.type = type; } + + void update_radio_status(const radio_status_s &radio_status); + + unsigned get_system_type() { return _param_mav_type.get(); } + + Protocol get_protocol() const { return _protocol; } + + int get_socket_fd() { return _socket_fd; }; + + bool _task_should_exit{false}; /**< Mavlink task should exit iff true. */ + +#if defined(MAVLINK_UDP) + unsigned short get_network_port() { return _network_port; } + + unsigned short get_remote_port() { return _remote_port; } + + const in_addr query_netmask_addr(const int socket_fd, const ifreq &ifreq); + + const in_addr compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask_addr); + + struct sockaddr_in &get_client_source_address() { return _src_addr; } + + void set_client_source_initialized() { _src_addr_initialized = true; } + + bool get_client_source_initialized() { return _src_addr_initialized; } +#endif + + uint64_t get_start_time() { return _mavlink_start_time; } + + static bool boot_complete() { return _boot_complete; } + + bool is_usb_uart() { return _is_usb_uart; } + + int get_data_rate() { return _datarate; } + void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } } + + unsigned get_main_loop_delay() const { return _main_loop_delay; } + + /** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread. + * Returns nullptr if shell cannot be created */ + MavlinkShell *get_shell(); + /** close the Mavlink shell if it is open */ + void close_shell(); + + /** get ulog streaming if active, nullptr otherwise */ + MavlinkULog *get_ulog_streaming() { return _mavlink_ulog; } + void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component) + { + if (_mavlink_ulog) { return; } + + _mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component); + } + void request_stop_ulog_streaming() + { + if (_mavlink_ulog) { _mavlink_ulog_stop_requested.store(true); } + } + + bool ftp_enabled() const { return _ftp_on; } + + bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); } + bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); } + bool odometry_loopback_enabled() const { return _param_mav_odom_lp.get(); } + + bool failure_injection_enabled() const { return _param_sys_failure_injection_enabled.get(); } + + struct ping_statistics_s { + uint64_t last_ping_time; + uint32_t last_ping_seq; + uint32_t dropped_packets; + float last_rtt; + float mean_rtt; + float max_rtt; + float min_rtt; + }; + + /** + * Get the ping statistics of this MAVLink link + */ + struct ping_statistics_s &get_ping_statistics() { return _ping_stats; } + + static hrt_abstime &get_first_start_time() { return _first_start_time; } + + bool radio_status_critical() const { return _radio_status_critical; } + +private: + MavlinkReceiver _receiver; + int _instance_id{-1}; + + bool _transmitting_enabled{true}; + bool _transmitting_enabled_commanded{false}; + bool _first_heartbeat_sent{false}; + + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationMulti _telemetry_status_pub{ORB_ID(telemetry_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + bool _task_running{true}; + static bool _boot_complete; + + static constexpr int MAVLINK_MIN_INTERVAL{1500}; + static constexpr int MAVLINK_MAX_INTERVAL{10000}; + static constexpr float MAVLINK_MIN_MULTIPLIER{0.0005f}; + + mavlink_message_t _mavlink_buffer {}; + mavlink_status_t _mavlink_status {}; + + /* states */ + bool _hil_enabled{false}; /**< Hardware In the Loop mode */ + bool _generate_rc{false}; /**< Generate RC messages from manual input MAVLink messages */ + bool _is_usb_uart{false}; /**< Port is USB */ + bool _wait_to_transmit{false}; /**< Wait to transmit until received messages. */ + bool _received_messages{false}; /**< Whether we've received valid mavlink messages. */ + + unsigned _main_loop_delay{1000}; /**< mainloop delay, depends on data rate */ + + List _streams; + + MavlinkShell *_mavlink_shell{nullptr}; + MavlinkULog *_mavlink_ulog{nullptr}; + + px4::atomic_bool _mavlink_ulog_stop_requested{false}; + + MAVLINK_MODE _mode{MAVLINK_MODE_NORMAL}; + + mavlink_channel_t _channel{MAVLINK_COMM_0}; + + bool _forwarding_on{false}; + bool _ftp_on{false}; + bool _use_software_mav_throttling{false}; + + int _uart_fd{-1}; + + int _baudrate{57600}; + int _datarate{1000}; ///< data rate for normal streams (attitude, position, etc.) + float _rate_mult{1.0f}; + + bool _radio_status_available{false}; + bool _radio_status_critical{false}; + float _radio_status_mult{1.0f}; + + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int _mavlink_param_queue_index{0}; + + bool _mavlink_link_termination_allowed{false}; + + char *_subscribe_to_stream{nullptr}; + float _subscribe_to_stream_rate{0.0f}; ///< rate of stream to subscribe to (0=disable, -1=unlimited, -2=default) + bool _udp_initialised{false}; + + FLOW_CONTROL_MODE _flow_control_mode{Mavlink::FLOW_CONTROL_OFF}; + + uint64_t _last_write_success_time{0}; + uint64_t _last_write_try_time{0}; + uint64_t _mavlink_start_time{0}; + int32_t _protocol_version_switch{-1}; + int32_t _protocol_version{0}; + + unsigned _bytes_tx{0}; + unsigned _bytes_txerr{0}; + unsigned _bytes_rx{0}; + hrt_abstime _bytes_timestamp{0}; + +#if defined(MAVLINK_UDP) + BROADCAST_MODE _mav_broadcast {BROADCAST_MODE_OFF}; + + sockaddr_in _myaddr {}; + sockaddr_in _src_addr {}; + sockaddr_in _bcast_addr {}; + + bool _src_addr_initialized{false}; + bool _broadcast_address_found{false}; + bool _broadcast_address_not_found_warned{false}; + bool _broadcast_failed_warned{false}; + + unsigned short _network_port{14556}; + unsigned short _remote_port{DEFAULT_REMOTE_PORT_UDP}; +#endif // MAVLINK_UDP + + uint8_t _buf[MAVLINK_MAX_PACKET_LEN] {}; + unsigned _buf_fill{0}; + + bool _tx_buffer_low{false}; + + const char *_interface_name{nullptr}; + + int _socket_fd{-1}; + Protocol _protocol{Protocol::SERIAL}; + + radio_status_s _rstatus {}; + telemetry_status_s _tstatus {}; + bool _tstatus_updated{false}; + + ping_statistics_s _ping_stats {}; + + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + + mavlink_message_buffer _message_buffer {}; + + pthread_mutex_t _message_buffer_mutex {}; + pthread_mutex_t _send_mutex {}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_comp_id, + (ParamInt) _param_mav_proto_ver, + (ParamInt) _param_sik_radio_id, + (ParamInt) _param_mav_type, + (ParamBool) _param_mav_usehilgps, + (ParamBool) _param_mav_fwdextsp, + (ParamBool) _param_mav_hash_chk_en, + (ParamBool) _param_mav_hb_forw_en, + (ParamBool) _param_mav_odom_lp, + (ParamInt) _param_mav_radio_timeout, + (ParamInt) _param_sys_hitl, + (ParamBool) _param_sys_failure_injection_enabled + ) + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */ + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": tx run interval")}; /**< loop interval performance counter */ + perf_counter_t _send_byte_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": send_bytes error")}; /**< send bytes error count */ + + void mavlink_update_parameters(); + + int mavlink_open_uart(const int baudrate = DEFAULT_BAUD_RATE, + const char *uart_name = DEFAULT_DEVICE_NAME, + const FLOW_CONTROL_MODE flow_control = FLOW_CONTROL_AUTO); + + static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25; + static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35; + static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50; + + static hrt_abstime _first_start_time; + + /** + * Configure a single stream. + * @param stream_name + * @param rate streaming rate in Hz, -1 = unlimited rate + * @return 0 on success, <0 on error + */ + int configure_stream(const char *stream_name, const float rate = -1.0f); + + /** + * Configure default streams according to _mode for either all streams or only a single + * stream. + * @param configure_single_stream: if nullptr, configure all streams, else only a single stream + * @return 0 on success, <0 on error + */ + int configure_streams_to_default(const char *configure_single_stream = nullptr); + + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty() const { return (_message_buffer.read_ptr == _message_buffer.write_ptr); } + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n) { _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; } + + void pass_message(const mavlink_message_t *msg); + + void publish_telemetry_status(); + + void check_requested_subscriptions(); + + /** + * Reconfigure a SiK radio if requested by MAV_SIK_RADIO_ID + * + * This convenience function allows to re-configure a connected + * SiK radio without removing it from the main system harness. + */ + void configure_sik_radio(); + + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + +#if defined(MAVLINK_UDP) + void find_broadcast_address(); + + void init_udp(); +#endif // MAVLINK_UDP + + + void set_channel(); + + bool set_instance_id(); + + /** + * Main mavlink task. + */ + int task_main(int argc, char *argv[]); + + // Disallow copy construction and move assignment. + Mavlink(const Mavlink &) = delete; + Mavlink operator=(const Mavlink &) = delete; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_messages.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_messages.cpp new file mode 100644 index 0000000..dbfd97c --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_messages.cpp @@ -0,0 +1,578 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink_messages.cpp + * MAVLink 2.0 message formatters implementation. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +#include "mavlink_main.h" +#include "mavlink_messages.h" +#include "mavlink_command_sender.h" +#include "mavlink_simple_analyzer.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "streams/ACTUATOR_CONTROL_TARGET.hpp" +#include "streams/ACTUATOR_OUTPUT_STATUS.hpp" +#include "streams/ALTITUDE.hpp" +#include "streams/ATTITUDE.hpp" +#include "streams/ATTITUDE_QUATERNION.hpp" +#include "streams/ATTITUDE_TARGET.hpp" +#include "streams/AUTOPILOT_VERSION.hpp" +#include "streams/BATTERY_STATUS.hpp" +#include "streams/CAMERA_IMAGE_CAPTURED.hpp" +#include "streams/CAMERA_TRIGGER.hpp" +#include "streams/COLLISION.hpp" +#include "streams/COMMAND_LONG.hpp" +#include "streams/COMPONENT_INFORMATION.hpp" +#include "streams/DISTANCE_SENSOR.hpp" +#include "streams/ESC_INFO.hpp" +#include "streams/ESC_STATUS.hpp" +#include "streams/ESTIMATOR_STATUS.hpp" +#include "streams/EXTENDED_SYS_STATE.hpp" +#include "streams/FLIGHT_INFORMATION.hpp" +#include "streams/GLOBAL_POSITION_INT.hpp" +#include "streams/GPS_GLOBAL_ORIGIN.hpp" +#include "streams/GPS_RAW_INT.hpp" +#include "streams/GPS_STATUS.hpp" +#include "streams/HEARTBEAT.hpp" +#include "streams/HIGHRES_IMU.hpp" +#include "streams/HIL_ACTUATOR_CONTROLS.hpp" +#include "streams/HIL_STATE_QUATERNION.hpp" +#include "streams/HOME_POSITION.hpp" +#include "streams/LOCAL_POSITION_NED.hpp" +#include "streams/MANUAL_CONTROL.hpp" +#include "streams/MOUNT_ORIENTATION.hpp" +#include "streams/NAV_CONTROLLER_OUTPUT.hpp" +#include "streams/OBSTACLE_DISTANCE.hpp" +#include "streams/OPTICAL_FLOW_RAD.hpp" +#include "streams/ORBIT_EXECUTION_STATUS.hpp" +#include "streams/PING.hpp" +#include "streams/POSITION_TARGET_GLOBAL_INT.hpp" +#include "streams/POSITION_TARGET_LOCAL_NED.hpp" +#include "streams/PROTOCOL_VERSION.hpp" +#include "streams/RAW_RPM.hpp" +#include "streams/RC_CHANNELS.hpp" +#include "streams/SCALED_IMU.hpp" +#include "streams/SCALED_IMU2.hpp" +#include "streams/SCALED_IMU3.hpp" +#include "streams/SCALED_PRESSURE.hpp" +#include "streams/SERVO_OUTPUT_RAW.hpp" +#include "streams/STATUSTEXT.hpp" +#include "streams/STORAGE_INFORMATION.hpp" +#include "streams/SYS_STATUS.hpp" +#include "streams/SYSTEM_TIME.hpp" +#include "streams/TIMESYNC.hpp" +#include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp" +#include "streams/VFR_HUD.hpp" +#include "streams/VIBRATION.hpp" +#include "streams/WIND_COV.hpp" + +#if !defined(CONSTRAINED_FLASH) +# include "streams/ADSB_VEHICLE.hpp" +# include "streams/ATT_POS_MOCAP.hpp" +# include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp" +# include "streams/DEBUG.hpp" +# include "streams/DEBUG_FLOAT_ARRAY.hpp" +# include "streams/DEBUG_VECT.hpp" +# include "streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp" +# include "streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp" +# include "streams/GIMBAL_MANAGER_INFORMATION.hpp" +# include "streams/GIMBAL_MANAGER_STATUS.hpp" +# include "streams/GPS2_RAW.hpp" +# include "streams/HIGH_LATENCY2.hpp" +# include "streams/LINK_NODE_STATUS.hpp" +# include "streams/NAMED_VALUE_FLOAT.hpp" +# include "streams/ODOMETRY.hpp" +# include "streams/SCALED_PRESSURE2.hpp" +# include "streams/SCALED_PRESSURE3.hpp" +# include "streams/SMART_BATTERY_INFO.hpp" +# include "streams/UTM_GLOBAL_POSITION.hpp" +#endif // !CONSTRAINED_FLASH + +// ensure PX4 rotation enum and MAV_SENSOR_ROTATION align +static_assert(MAV_SENSOR_ROTATION_NONE == static_cast(ROTATION_NONE), + "Roll: 0, Pitch: 0, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_YAW_45 == static_cast(ROTATION_YAW_45), + "Roll: 0, Pitch: 0, Yaw: 45"); +static_assert(MAV_SENSOR_ROTATION_YAW_90 == static_cast(ROTATION_YAW_90), + "Roll: 0, Pitch: 0, Yaw: 90"); +static_assert(MAV_SENSOR_ROTATION_YAW_135 == static_cast(ROTATION_YAW_135), + "Roll: 0, Pitch: 0, Yaw: 135"); +static_assert(MAV_SENSOR_ROTATION_YAW_180 == static_cast(ROTATION_YAW_180), + "Roll: 0, Pitch: 0, Yaw: 180"); +static_assert(MAV_SENSOR_ROTATION_YAW_225 == static_cast(ROTATION_YAW_225), + "Roll: 0, Pitch: 0, Yaw: 225"); +static_assert(MAV_SENSOR_ROTATION_YAW_270 == static_cast(ROTATION_YAW_270), + "Roll: 0, Pitch: 0, Yaw: 270"); +static_assert(MAV_SENSOR_ROTATION_YAW_315 == static_cast(ROTATION_YAW_315), + "Roll: 0, Pitch: 0, Yaw: 315"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180 == static_cast(ROTATION_ROLL_180), + "Roll: 180, Pitch: 0, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180_YAW_45 == static_cast(ROTATION_ROLL_180_YAW_45), + "Roll: 180, Pitch: 0, Yaw: 45"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180_YAW_90 == static_cast(ROTATION_ROLL_180_YAW_90), + "Roll: 180, Pitch: 0, Yaw: 90"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180_YAW_135 == static_cast(ROTATION_ROLL_180_YAW_135), + "Roll: 180, Pitch: 0, Yaw: 135"); +static_assert(MAV_SENSOR_ROTATION_PITCH_180 == static_cast(ROTATION_PITCH_180), + "Roll: 0, Pitch: 180, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180_YAW_225 == static_cast(ROTATION_ROLL_180_YAW_225), + "Roll: 180, Pitch: 0, Yaw: 225"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180_YAW_270 == static_cast(ROTATION_ROLL_180_YAW_270), + "Roll: 180, Pitch: 0, Yaw: 270"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180_YAW_315 == static_cast(ROTATION_ROLL_180_YAW_315), + "Roll: 180, Pitch: 0, Yaw: 315"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90 == static_cast(ROTATION_ROLL_90), + "Roll: 90, Pitch: 0, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_YAW_45 == static_cast(ROTATION_ROLL_90_YAW_45), + "Roll: 90, Pitch: 0, Yaw: 45"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_YAW_90 == static_cast(ROTATION_ROLL_90_YAW_90), + "Roll: 90, Pitch: 0, Yaw: 90"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_YAW_135 == static_cast(ROTATION_ROLL_90_YAW_135), + "Roll: 90, Pitch: 0, Yaw: 135"); +static_assert(MAV_SENSOR_ROTATION_ROLL_270 == static_cast(ROTATION_ROLL_270), + "Roll: 270, Pitch: 0, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_270_YAW_45 == static_cast(ROTATION_ROLL_270_YAW_45), + "Roll: 270, Pitch: 0, Yaw: 45"); +static_assert(MAV_SENSOR_ROTATION_ROLL_270_YAW_90 == static_cast(ROTATION_ROLL_270_YAW_90), + "Roll: 270, Pitch: 0, Yaw: 90"); +static_assert(MAV_SENSOR_ROTATION_ROLL_270_YAW_135 == static_cast(ROTATION_ROLL_270_YAW_135), + "Roll: 270, Pitch: 0, Yaw: 135"); +static_assert(MAV_SENSOR_ROTATION_PITCH_90 == static_cast(ROTATION_PITCH_90), + "Roll: 0, Pitch: 90, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_PITCH_270 == static_cast(ROTATION_PITCH_270), + "Roll: 0, Pitch: 270, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_PITCH_180_YAW_90 == static_cast(ROTATION_PITCH_180_YAW_90), + "Roll: 0, Pitch: 180, Yaw: 90"); +static_assert(MAV_SENSOR_ROTATION_PITCH_180_YAW_270 == static_cast(ROTATION_PITCH_180_YAW_270), + "Roll: 0, Pitch: 180, Yaw: 270"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_90 == static_cast(ROTATION_ROLL_90_PITCH_90), + "Roll: 90, Pitch: 90, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180_PITCH_90 == static_cast(ROTATION_ROLL_180_PITCH_90), + "Roll: 180, Pitch: 90, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_270_PITCH_90 == static_cast(ROTATION_ROLL_270_PITCH_90), + "Roll: 270, Pitch: 90, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_180 == static_cast(ROTATION_ROLL_90_PITCH_180), + "Roll: 90, Pitch: 180, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_270_PITCH_180 == static_cast + (ROTATION_ROLL_270_PITCH_180), "Roll: 270, Pitch: 180, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_270 == static_cast(ROTATION_ROLL_90_PITCH_270), + "Roll: 90, Pitch: 270, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_180_PITCH_270 == static_cast + (ROTATION_ROLL_180_PITCH_270), "Roll: 180, Pitch: 270, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_270_PITCH_270 == static_cast + (ROTATION_ROLL_270_PITCH_270), "Roll: 270, Pitch: 270, Yaw: 0"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_180_YAW_90 == static_cast + (ROTATION_ROLL_90_PITCH_180_YAW_90), + "Roll: 90, Pitch: 180, Yaw: 90"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_YAW_270 == static_cast(ROTATION_ROLL_90_YAW_270), + "Roll: 90, Pitch: 0, Yaw: 270"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293 == static_cast + (ROTATION_ROLL_90_PITCH_68_YAW_293), + "Roll: 90, Pitch: 68, Yaw: 293"); +static_assert(MAV_SENSOR_ROTATION_PITCH_315 == static_cast(ROTATION_PITCH_315), "Pitch: 315"); +static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_315 == static_cast(ROTATION_ROLL_90_PITCH_315), + "Roll: 90, Pitch: 315"); +static_assert(41 == ROTATION_MAX, "Keep MAV_SENSOR_ROTATION and PX4 Rotation in sync"); + + +union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) +{ + union px4_custom_mode custom_mode; + custom_mode.data = 0; + + switch (nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; + break; + + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; + + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND; + break; + + case vehicle_status_s::NAVIGATION_STATE_ORBIT: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT; + break; + } + + return custom_mode; +} + +static const StreamListItem streams_list[] = { +#if defined(HEARTBEAT_HPP) + create_stream_list_item(), +#endif // HEARTBEAT_HPP +#if defined(STATUSTEXT_HPP) + create_stream_list_item(), +#endif // STATUSTEXT_HPP +#if defined(COMMAND_LONG_HPP) + create_stream_list_item(), +#endif // COMMAND_LONG_HPP +#if defined(SYSTEM_TIME_HPP) + create_stream_list_item(), +#endif // SYSTEM_TIME_HPP + create_stream_list_item(), +#if defined(SMART_BATTERY_INFO_HPP) + create_stream_list_item(), +#endif // SMART_BATTERY_INFO_HPP +#if defined(HIGHRES_IMU_HPP) + create_stream_list_item(), +#endif // HIGHRES_IMU_HPP +#if defined(SCALED_IMU_HPP) + create_stream_list_item(), +#endif // SCALED_IMU_HPP +#if defined(SCALED_IMU2_HPP) + create_stream_list_item(), +#endif // SCALED_IMU2_HPP +#if defined(SCALED_IMU3_HPP) + create_stream_list_item(), +#endif // SCALED_IMU3_HPP +#if defined(SCALED_PRESSURE) + create_stream_list_item(), +#endif // SCALED_PRESSURE +#if defined(SCALED_PRESSURE2) + create_stream_list_item(), +#endif // SCALED_PRESSURE2 +#if defined(SCALED_PRESSURE3) + create_stream_list_item(), +#endif // SCALED_PRESSURE3 +#if defined(ACTUATOR_OUTPUT_STATUS_HPP) + create_stream_list_item(), +#endif // ACTUATOR_OUTPUT_STATUS_HPP +#if defined(ATTITUDE_HPP) + create_stream_list_item(), +#endif // ATTITUDE_HPP +#if defined(ATTITUDE_QUATERNION_HPP) + create_stream_list_item(), +#endif // ATTITUDE_QUATERNION_HPP +#if defined(VFR_HUD_HPP) + create_stream_list_item(), +#endif // VFR_HUD_HPP +#if defined(GPS_GLOBAL_ORIGIN_HPP) + create_stream_list_item(), +#endif // GPS_GLOBAL_ORIGIN_HPP +#if defined(GPS_RAW_INT_HPP) + create_stream_list_item(), +#endif // GPS_RAW_INT_HPP +#if defined(GPS2_RAW_HPP) + create_stream_list_item(), +#endif // GPS2_RAW_HPP +#if defined(SYSTEM_TIME_HPP) + create_stream_list_item(), +#endif // SYSTEM_TIME_HPP +#if defined(TIMESYNC_HPP) + create_stream_list_item(), +#endif // TIMESYNC_HPP +#if defined(GLOBAL_POSITION_INT_HPP) + create_stream_list_item(), +#endif // GLOBAL_POSITION_INT_HPP +#if defined(LOCAL_POSITION_NED_HPP) + create_stream_list_item(), +#endif // LOCAL_POSITION_NED_HPP +#if defined(ODOMETRY_HPP) + create_stream_list_item(), +#endif // ODOMETRY_HPP +#if defined(ESTIMATOR_STATUS_HPP) + create_stream_list_item(), +#endif // ESTIMATOR_STATUS_HPP +#if defined(VIBRATION_HPP) + create_stream_list_item(), +#endif // VIBRATION_HPP +#if defined(ATT_POS_MOCAP_HPP) + create_stream_list_item(), +#endif // ATT_POS_MOCAP_HPP +#if defined(AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP) + create_stream_list_item(), +#endif // AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP +#if defined(GIMBAL_DEVICE_ATTITUDE_STATUS_HPP) + create_stream_list_item(), +#endif // GIMBAL_DEVICE_ATTITUDE_STATUS_HPP +#if defined(GIMBAL_MANAGER_INFORMATION_HPP) + create_stream_list_item(), +#endif // GIMBAL_MANAGER_INFORMATION_HPP +#if defined(GIMBAL_MANAGER_STATUS_HPP) + create_stream_list_item(), +#endif // GIMBAL_MANAGER_STATUS_HPP +#if defined(GIMBAL_DEVICE_SET_ATTITUDE_HPP) + create_stream_list_item(), +#endif // GIMBAL_DEVICE_SET_ATTITUDE_HPP +#if defined(HOME_POSITION_HPP) + create_stream_list_item(), +#endif // HOME_POSITION_HPP +#if defined(SERVO_OUTPUT_RAW_HPP) + create_stream_list_item >(), + create_stream_list_item >(), +#endif // SERVO_OUTPUT_RAW_HPP +#if defined(HIL_ACTUATOR_CONTROLS_HPP) + create_stream_list_item(), +#endif // HIL_ACTUATOR_CONTROLS_HPP +#if defined(POSITION_TARGET_GLOBAL_INT_HPP) + create_stream_list_item(), +#endif // POSITION_TARGET_GLOBAL_INT_HPP +#if defined(POSITION_TARGET_LOCAL_NED_HPP) + create_stream_list_item(), +#endif // POSITION_TARGET_LOCAL_NED_HPP +#if defined(ATTITUDE_TARGET_HPP) + create_stream_list_item(), +#endif // ATTITUDE_TARGET_HPP +#if defined(RC_CHANNELS_HPP) + create_stream_list_item(), +#endif // RC_CHANNELS_HPP +#if defined(MANUAL_CONTROL_HPP) + create_stream_list_item(), +#endif // MANUAL_CONTROL_HPP +#if defined(TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP) + create_stream_list_item(), +#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP +#if defined(OPTICAL_FLOW_RAD_HPP) + create_stream_list_item(), +#endif // OPTICAL_FLOW_RAD_HPP +#if defined(ACTUATOR_CONTROL_TARGET_HPP) + create_stream_list_item >(), + create_stream_list_item >(), +#endif // ACTUATOR_CONTROL_TARGET_HPP +#if defined(NAMED_VALUE_FLOAT_HPP) + create_stream_list_item(), +#endif // NAMED_VALUE_FLOAT_HPP +#if defined(DEBUG_HPP) + create_stream_list_item(), +#endif // DEBUG_HPP +#if defined(DEBUG_VECT_HPP) + create_stream_list_item(), +#endif // DEBUG_VECT_HPP +#if defined(DEBUG_FLOAT_ARRAY_HPP) + create_stream_list_item(), +#endif // DEBUG_FLOAT_ARRAY_HPP +#if defined(NAV_CONTROLLER_OUTPUT_HPP) + create_stream_list_item(), +#endif // NAV_CONTROLLER_OUTPUT_HPP +#if defined(CAMERA_TRIGGER_HPP) + create_stream_list_item(), +#endif // CAMERA_TRIGGER_HPP +#if defined(CAMERA_IMAGE_CAPTURED_HPP) + create_stream_list_item(), +#endif // CAMERA_IMAGE_CAPTURED_HPP +#if defined(DISTANCE_SENSOR_HPP) + create_stream_list_item(), +#endif // DISTANCE_SENSOR_HPP +#if defined(EXTENDED_SYS_STATE_HPP) + create_stream_list_item(), +#endif // EXTENDED_SYS_STATE_HPP +#if defined(ALTITUDE_HPP) + create_stream_list_item(), +#endif // ALTITUDE_HPP +#if defined(ADSB_VEHICLE_HPP) + create_stream_list_item(), +#endif // ADSB_VEHICLE_HPP +#if defined(UTM_GLOBAL_POSITION_HPP) + create_stream_list_item(), +#endif // UTM_GLOBAL_POSITION_HPP +#if defined(COLLISION_HPP) + create_stream_list_item(), +#endif // COLLISION_HPP +#if defined(WIND_COV_HPP) + create_stream_list_item(), +#endif // WIND_COV_HPP +#if defined(MOUNT_ORIENTATION_HPP) + create_stream_list_item(), +#endif // MOUNT_ORIENTATION_HPP +#if defined(HIGH_LATENCY2_HPP) + create_stream_list_item(), +#endif // HIGH_LATENCY2_HPP +#if defined(HIL_STATE_QUATERNION_HPP) + create_stream_list_item(), +#endif // HIL_STATE_QUATERNION_HPP +#if defined(PING_HPP) + create_stream_list_item(), +#endif // PING_HPP +#if defined(ORBIT_EXECUTION_STATUS_HPP) + create_stream_list_item(), +#endif // ORBIT_EXECUTION_STATUS_HPP +#if defined(OBSTACLE_DISTANCE_HPP) + create_stream_list_item(), +#endif // OBSTACLE_DISTANCE_HPP +#if defined(ESC_INFO_HPP) + create_stream_list_item(), +#endif // ESC_INFO_HPP +#if defined(ESC_STATUS_HPP) + create_stream_list_item(), +#endif // ESC_STATUS_HPP +#if defined(AUTOPILOT_VERSION_HPP) + create_stream_list_item(), +#endif // AUTOPILOT_VERSION_HPP +#if defined(PROTOCOL_VERSION_HPP) + create_stream_list_item(), +#endif // PROTOCOL_VERSION_HPP +#if defined(FLIGHT_INFORMATION_HPP) + create_stream_list_item(), +#endif // FLIGHT_INFORMATION_HPP +#if defined(GPS_STATUS_HPP) + create_stream_list_item(), +#endif // GPS_STATUS_HPP +#if defined(LINK_NODE_STATUS_HPP) + create_stream_list_item(), +#endif // LINK_NODE_STATUS_HPP +#if defined(STORAGE_INFORMATION_HPP) + create_stream_list_item(), +#endif // STORAGE_INFORMATION_HPP +#if defined(COMPONENT_INFORMATION_HPP) + create_stream_list_item(), +#endif // COMPONENT_INFORMATION_HPP +#if defined(RAW_RPM_HPP) + create_stream_list_item() +#endif // RAW_RPM_HPP +}; + +const char *get_stream_name(const uint16_t msg_id) +{ + // search for stream with specified msg id in supported streams list + for (const auto &stream : streams_list) { + if (msg_id == stream.get_id()) { + return stream.get_name(); + } + } + + return nullptr; +} + +MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink) +{ + // search for stream with specified name in supported streams list + if (stream_name != nullptr) { + for (const auto &stream : streams_list) { + if (strcmp(stream_name, stream.get_name()) == 0) { + return stream.new_instance(mavlink); + } + } + } + + return nullptr; +} + +MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink) +{ + // search for stream with specified name in supported streams list + for (const auto &stream : streams_list) { + if (msg_id == stream.get_id()) { + return stream.new_instance(mavlink); + } + } + + return nullptr; +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_messages.h b/src/prometheus_px4/src/modules/mavlink/mavlink_messages.h new file mode 100644 index 0000000..21cef65 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_messages.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink_messages.h + * MAVLink 1.0 message formatters definition. + * + * @author Anton Babushkin + */ + +#ifndef MAVLINK_MESSAGES_H_ +#define MAVLINK_MESSAGES_H_ + +#include "mavlink_stream.h" + +#include + +class StreamListItem +{ + +public: + MavlinkStream *(*new_instance)(Mavlink *mavlink); + const char *name; + uint16_t id; + + StreamListItem(MavlinkStream * (*inst)(Mavlink *mavlink), const char *_name, uint16_t _id) : + new_instance(inst), + name(_name), + id(_id) {} + + const char *get_name() const { return name; } + uint16_t get_id() const { return id; } +}; + +template +static StreamListItem create_stream_list_item() +{ + return StreamListItem(&T::new_instance, T::get_name_static(), T::get_id_static()); +} + +const char *get_stream_name(const uint16_t msg_id); + +MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink); + +MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink); + +union px4_custom_mode get_px4_custom_mode(uint8_t nav_state); + +#endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_mission.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_mission.cpp new file mode 100644 index 0000000..2ee5dfa --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_mission.cpp @@ -0,0 +1,1708 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink_mission.cpp + * MAVLink mission manager implementation. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "mavlink_mission.h" +#include "mavlink_main.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::wrap_2pi; + +dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; +bool MavlinkMissionManager::_dataman_init = false; +uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 }; +int32_t MavlinkMissionManager::_current_seq = 0; +bool MavlinkMissionManager::_transfer_in_progress = false; +constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; +uint16_t MavlinkMissionManager::_geofence_update_counter = 0; +uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; + + +#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ + ((_msg.target_component == mavlink_system.compid) || \ + (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ + (_msg.target_component == MAV_COMP_ID_ALL))) + +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : + _mavlink(mavlink) +{ + init_offboard_mission(); +} + +void +MavlinkMissionManager::init_offboard_mission() +{ + if (!_dataman_init) { + _dataman_init = true; + + /* lock MISSION_STATE item */ + int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + + if (dm_lock_ret != 0) { + PX4_ERR("DM_KEY_MISSION_STATE lock failed"); + } + + mission_s mission_state; + int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + /* unlock MISSION_STATE item */ + if (dm_lock_ret == 0) { + dm_unlock(DM_KEY_MISSION_STATE); + } + + if (ret > 0) { + _dataman_id = (dm_item_t)mission_state.dataman_id; + _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; + _current_seq = mission_state.current_seq; + + } else if (ret < 0) { + PX4_ERR("offboard mission init failed (%i)", ret); + } + + load_geofence_stats(); + + load_safepoint_stats(); + } + + _my_dataman_id = _dataman_id; +} + +int +MavlinkMissionManager::load_geofence_stats() +{ + mission_stats_entry_s stats; + // initialize fence points count + int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + + if (ret == sizeof(mission_stats_entry_s)) { + _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; + _geofence_update_counter = stats.update_counter; + } + + return ret; +} + +int +MavlinkMissionManager::load_safepoint_stats() +{ + mission_stats_entry_s stats; + // initialize safe points count + int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + + if (ret == sizeof(mission_stats_entry_s)) { + _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; + } + + return ret; +} + +/** + * Publish mission topic to notify navigator about changes. + */ +int +MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq) +{ + // We want to make sure the whole struct is initialized including padding before getting written by dataman. + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = dataman_id; + mission.count = count; + mission.current_seq = seq; + + /* update mission state in dataman */ + + /* lock MISSION_STATE item */ + int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + + if (dm_lock_ret != 0) { + PX4_ERR("DM_KEY_MISSION_STATE lock failed"); + } + + int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + + /* unlock MISSION_STATE item */ + if (dm_lock_ret == 0) { + dm_unlock(DM_KEY_MISSION_STATE); + } + + if (res == sizeof(mission_s)) { + /* update active mission state */ + _dataman_id = dataman_id; + _count[MAV_MISSION_TYPE_MISSION] = count; + _current_seq = seq; + _my_dataman_id = _dataman_id; + + /* mission state saved successfully, publish offboard_mission topic */ + _offboard_mission_pub.publish(mission); + + return PX4_OK; + + } else { + PX4_ERR("WPM: can't save mission state"); + + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); + } + + return PX4_ERROR; + } +} +int +MavlinkMissionManager::update_geofence_count(unsigned count) +{ + mission_stats_entry_s stats; + stats.num_items = count; + stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data + + /* update stats in dataman */ + int res = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + + if (res == sizeof(mission_stats_entry_s)) { + _count[MAV_MISSION_TYPE_FENCE] = count; + + } else { + + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); + } + + return PX4_ERROR; + } + + return PX4_OK; +} + +int +MavlinkMissionManager::update_safepoint_count(unsigned count) +{ + mission_stats_entry_s stats; + stats.num_items = count; + stats.update_counter = ++_safepoint_update_counter; + + /* update stats in dataman */ + int res = dm_write(DM_KEY_SAFE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + + if (res == sizeof(mission_stats_entry_s)) { + _count[MAV_MISSION_TYPE_RALLY] = count; + + } else { + + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); + } + + return PX4_ERROR; + } + + return PX4_OK; +} + +void +MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_mission_ack_t wpa{}; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + wpa.mission_type = _mission_type; + + mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa); + + PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); +} + +void +MavlinkMissionManager::send_mission_current(int32_t seq) +{ + int32_t item_count = _count[MAV_MISSION_TYPE_MISSION]; + + if (seq < item_count) { + mavlink_mission_current_t wpc{}; + wpc.seq = seq; + mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc); + + } else if (seq <= 0 && item_count == 0) { + /* don't broadcast if no WPs */ + + } else { + PX4_DEBUG("WPM: Send MISSION_CURRENT ERROR: seq %d out of bounds", seq); + + _mavlink->send_statustext_critical("ERROR: wp index out of bounds"); + } +} + +void +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type) +{ + _time_last_sent = hrt_absolute_time(); + + mavlink_mission_count_t wpc{}; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = count; + wpc.mission_type = mission_type; + + mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc); + + PX4_DEBUG("WPM: Send MISSION_COUNT %u to ID %u, mission type=%i", wpc.count, wpc.target_system, mission_type); +} + +void +MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + mission_item_s mission_item{}; + int read_result = 0; + + switch (_mission_type) { + + case MAV_MISSION_TYPE_MISSION: { + read_result = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s); + } + break; + + case MAV_MISSION_TYPE_FENCE: { // Read a geofence point + mission_fence_point_s mission_fence_point; + read_result = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) == + sizeof(mission_fence_point_s); + + mission_item.nav_cmd = mission_fence_point.nav_cmd; + mission_item.frame = mission_fence_point.frame; + mission_item.lat = mission_fence_point.lat; + mission_item.lon = mission_fence_point.lon; + mission_item.altitude = mission_fence_point.alt; + + if (mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || + mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + mission_item.vertex_count = mission_fence_point.vertex_count; + + } else { + mission_item.circle_radius = mission_fence_point.circle_radius; + } + } + break; + + case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point + mission_safe_point_s mission_safe_point; + read_result = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)) == + sizeof(mission_safe_point_s); + + mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT; + mission_item.frame = mission_safe_point.frame; + mission_item.lat = mission_safe_point.lat; + mission_item.lon = mission_safe_point.lon; + mission_item.altitude = mission_safe_point.alt; + } + break; + + default: + _mavlink->send_statustext_critical("Received unknown mission type, abort."); + break; + } + + if (read_result > 0) { + _time_last_sent = hrt_absolute_time(); + + if (_int_mode) { + mavlink_mission_item_int_t wp{}; + format_mavlink_mission_item(&mission_item, reinterpret_cast(&wp)); + + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + wp.current = (_current_seq == seq) ? 1 : 0; + + mavlink_msg_mission_item_int_send_struct(_mavlink->get_channel(), &wp); + + PX4_DEBUG("WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system); + + } else { + mavlink_mission_item_t wp{}; + format_mavlink_mission_item(&mission_item, &wp); + + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + wp.current = (_current_seq == seq) ? 1 : 0; + + mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp); + + PX4_DEBUG("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); + } + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + + if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { + _mavlink->send_statustext_critical("Mission storage: Unable to read from microSD"); + } + + PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); + } +} + +uint16_t +MavlinkMissionManager::current_max_item_count() +{ + if (_mission_type >= sizeof(MAX_COUNT) / sizeof(MAX_COUNT[0])) { + PX4_ERR("WPM: MAX_COUNT out of bounds (%u)", _mission_type); + return 0; + } + + return MAX_COUNT[_mission_type]; +} + +uint16_t +MavlinkMissionManager::current_item_count() +{ + if (_mission_type >= sizeof(_count) / sizeof(_count[0])) { + PX4_ERR("WPM: _count out of bounds (%u)", _mission_type); + return 0; + } + + return _count[_mission_type]; +} + +void +MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < current_max_item_count()) { + _time_last_sent = hrt_absolute_time(); + + if (_int_mode) { + mavlink_mission_request_int_t wpr{}; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + wpr.mission_type = _mission_type; + mavlink_msg_mission_request_int_send_struct(_mavlink->get_channel(), &wpr); + + PX4_DEBUG("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system); + + } else { + + mavlink_mission_request_t wpr{}; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + wpr.mission_type = _mission_type; + + mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr); + + PX4_DEBUG("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); + } + + } else { + _mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity"); + + PX4_DEBUG("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); + } +} + +void +MavlinkMissionManager::send_mission_item_reached(uint16_t seq) +{ + mavlink_mission_item_reached_t wp_reached{}; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached); + + PX4_DEBUG("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); +} + +void +MavlinkMissionManager::send() +{ + // do not send anything over high latency communication + if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { + return; + } + + mission_result_s mission_result{}; + + if (_mission_result_sub.update(&mission_result)) { + + if (_current_seq != mission_result.seq_current) { + _current_seq = mission_result.seq_current; + + PX4_DEBUG("WPM: got mission result, new current_seq: %u", _current_seq); + } + + if (_last_reached != mission_result.seq_reached) { + _last_reached = mission_result.seq_reached; + _reached_sent_count = 0; + + if (_last_reached >= 0) { + send_mission_item_reached((uint16_t)mission_result.seq_reached); + } + + PX4_DEBUG("WPM: got mission result, new seq_reached: %d", _last_reached); + } + + send_mission_current(_current_seq); + + if (mission_result.item_do_jump_changed) { + /* Send a mission item again if the remaining DO_JUMPs has changed, but don't interfere + * if there are ongoing transfers happening already. */ + if (_state == MAVLINK_WPM_STATE_IDLE) { + _mission_type = MAV_MISSION_TYPE_MISSION; + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, + (uint16_t)mission_result.item_changed_index); + } + } + + } else { + if (_slow_rate_limiter.check(hrt_absolute_time())) { + send_mission_current(_current_seq); + + // send the reached message another 10 times + if (_last_reached >= 0 && (_reached_sent_count < 10)) { + send_mission_item_reached((uint16_t)_last_reached); + _reached_sent_count++; + } + } + } + + /* check for timed-out operations */ + if (_state == MAVLINK_WPM_STATE_GETLIST && (_time_last_sent > 0) + && hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) { + + // try to request item again after timeout + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0) + && hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) { + + _mavlink->send_statustext_critical("Operation timeout"); + + PX4_DEBUG("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); + + switch_to_idle_state(); + + // since we are giving up, reset this state also, so another request can be started. + _transfer_in_progress = false; + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + // reset flags + _time_last_sent = 0; + _time_last_recv = 0; + } +} + + +void +MavlinkMissionManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_ACK: + handle_mission_ack(msg); + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + handle_mission_set_current(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + handle_mission_request_list(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + handle_mission_request(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_INT: + handle_mission_request_int(msg); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + handle_mission_count(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + handle_mission_item(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + handle_mission_item_int(msg); + break; + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + handle_mission_clear_all(msg); + break; + + default: + break; + } +} + +void +MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) +{ + mavlink_mission_ack_t wpa{}; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if (CHECK_SYSID_COMPID_MISSION(wpa)) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if (_state == MAVLINK_WPM_STATE_SENDLIST && _mission_type == wpa.mission_type) { + + _time_last_recv = hrt_absolute_time(); + + if (wpa.type == MAV_MISSION_ACCEPTED && _transfer_seq == current_item_count()) { + PX4_DEBUG("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); + + } else if (wpa.type == MAV_MISSION_OPERATION_CANCELLED) { + PX4_DEBUG("WPM: MISSION_ACK CANCELLED, switch to state IDLE"); + + } else { + PX4_DEBUG("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); + } + + switch_to_idle_state(); + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + + // INT or float mode is not supported + if (wpa.type == MAV_MISSION_UNSUPPORTED) { + + if (_int_mode) { + _int_mode = false; + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else { + _int_mode = true; + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } + + } else if (wpa.type == MAV_MISSION_OPERATION_CANCELLED) { + PX4_DEBUG("WPM: MISSION_ACK CANCELLED, switch to state IDLE"); + switch_to_idle_state(); + _transfer_in_progress = false; + + } else if (wpa.type != MAV_MISSION_ACCEPTED) { + PX4_WARN("Mission ack result was %d", wpa.type); + } + } + + } else { + _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); + + PX4_DEBUG("WPM: MISSION_ACK ERR: ID mismatch"); + } + } +} + +void +MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) +{ + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (CHECK_SYSID_COMPID_MISSION(wpc)) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) { + if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) { + PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); + + } else { + PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); + + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID"); + } + + } else { + PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); + + _mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list"); + } + + } else { + PX4_DEBUG("WPM: MISSION_SET_CURRENT ERROR: busy"); + + _mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) +{ + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (CHECK_SYSID_COMPID_MISSION(wprl)) { + const bool maybe_completed = (_transfer_seq == current_item_count()); + + // If all mission items have been sent and a new mission request list comes in, we can proceed even if MISSION_ACK was + // never received. This could happen on a quick reconnect that doesn't trigger MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT + if (maybe_completed) { + switch_to_idle_state(); + } + + if (_state == MAVLINK_WPM_STATE_IDLE || (_state == MAVLINK_WPM_STATE_SENDLIST + && (uint8_t)_mission_type == wprl.mission_type)) { + _time_last_recv = hrt_absolute_time(); + + _state = MAVLINK_WPM_STATE_SENDLIST; + _mission_type = (MAV_MISSION_TYPE)wprl.mission_type; + + // make sure our item counts are up-to-date + switch (_mission_type) { + case MAV_MISSION_TYPE_FENCE: + load_geofence_stats(); + break; + + case MAV_MISSION_TYPE_RALLY: + load_safepoint_stats(); + break; + + default: + break; + } + + _transfer_seq = 0; + _transfer_count = current_item_count(); + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + + if (_transfer_count > 0) { + PX4_DEBUG("WPM: MISSION_REQUEST_LIST OK, %u mission items to send, mission type=%i", _transfer_count, _mission_type); + + } else { + PX4_DEBUG("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", _mission_type); + } + + send_mission_count(msg->sysid, msg->compid, _transfer_count, _mission_type); + + } else { + PX4_DEBUG("WPM: MISSION_REQUEST_LIST ERROR: busy"); + + _mavlink->send_statustext_info("Mission download request ignored, already active"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) +{ + // The request comes in the old float mode, so we switch to it. + if (_int_mode) { + _int_mode = false; + } + + handle_mission_request_both(msg); +} + +void +MavlinkMissionManager::handle_mission_request_int(const mavlink_message_t *msg) +{ + // The request comes in the new int mode, so we switch to it. + if (!_int_mode) { + _int_mode = true; + } + + handle_mission_request_both(msg); +} + +void +MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) +{ + /* The mavlink_message_t could also be a mavlink_mission_request_int_t, however the structs + * are basically the same, so we can ignore it. */ + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (CHECK_SYSID_COMPID_MISSION(wpr)) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + + if (_mission_type != wpr.mission_type) { + PX4_WARN("WPM: Unexpected mission type (%u %u)", wpr.mission_type, _mission_type); + return; + } + + _time_last_recv = hrt_absolute_time(); + + /* _transfer_seq contains sequence of expected request */ + if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) { + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u", wpr.seq, msg->sysid); + + _transfer_seq++; + + } else if (wpr.seq == _transfer_seq - 1) { + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u (again)", wpr.seq, msg->sysid); + + } else { + if (_transfer_seq > 0 && _transfer_seq < _transfer_count) { + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, + _transfer_seq - 1, _transfer_seq); + + } else if (_transfer_seq <= 0) { + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, + _transfer_seq); + + } else { + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, + _transfer_seq - 1); + } + + switch_to_idle_state(); + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); + return; + } + + /* double check bounds in case of items count changed */ + if (wpr.seq < current_item_count()) { + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); + + } else { + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", wpr.seq, wpr.seq, + current_item_count() - 1); + + switch_to_idle_state(); + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected"); + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: no transfer"); + + // Silently ignore this as some OSDs have buggy mission protocol implementations + //_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST(_INT): No active transfer"); + + } else { + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: busy (state %d).", _state); + + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); + } + + } else { + _mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch"); + + PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: rejected, partner ID mismatch"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) +{ + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (CHECK_SYSID_COMPID_MISSION(wpc)) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_in_progress) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + return; + } + + _transfer_in_progress = true; + _mission_type = (MAV_MISSION_TYPE)wpc.mission_type; + + if (wpc.count > current_max_item_count()) { + PX4_DEBUG("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count()); + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); + _transfer_in_progress = false; + return; + } + + if (wpc.count == 0) { + PX4_DEBUG("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); + + switch (_mission_type) { + case MAV_MISSION_TYPE_MISSION: + + /* alternate dataman ID anyway to let navigator know about changes */ + + if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) { + update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0); + + } else { + update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + } + + break; + + case MAV_MISSION_TYPE_FENCE: + update_geofence_count(0); + break; + + case MAV_MISSION_TYPE_RALLY: + update_safepoint_count(0); + break; + + default: + PX4_ERR("mission type %u not handled", _mission_type); + break; + } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + _transfer_in_progress = false; + return; + } + + PX4_DEBUG("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + + _state = MAVLINK_WPM_STATE_GETLIST; + _transfer_seq = 0; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + _transfer_count = wpc.count; + _transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission + _transfer_current_seq = -1; + + if (_mission_type == MAV_MISSION_TYPE_FENCE) { + // We're about to write new geofence items, so take the lock. It will be released when + // switching back to idle + PX4_DEBUG("locking fence dataman items"); + + int ret = dm_lock(DM_KEY_FENCE_POINTS); + + if (ret == 0) { + _geofence_locked = true; + + } else { + PX4_ERR("locking failed (%i)", errno); + } + } + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == 0) { + /* looks like our MISSION_REQUEST was lost, try again */ + PX4_DEBUG("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); + + } else { + PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); + + _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + return; + } + + } else { + PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, state %i", _state); + + _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy"); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + return; + } + + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } +} + +void +MavlinkMissionManager::switch_to_idle_state() +{ + // when switching to idle, we *always* check if the lock was held and release it. + // This is to ensure we don't end up in a state where we forget to release it. + if (_geofence_locked) { + dm_unlock(DM_KEY_FENCE_POINTS); + _geofence_locked = false; + + PX4_DEBUG("unlocking geofence"); + } + + _state = MAVLINK_WPM_STATE_IDLE; +} + + +void +MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) +{ + if (_int_mode) { + // It seems that we should be using the float mode, let's switch out of int mode. + _int_mode = false; + } + + handle_mission_item_both(msg); +} + +void +MavlinkMissionManager::handle_mission_item_int(const mavlink_message_t *msg) +{ + if (!_int_mode) { + // It seems that we should be using the int mode, let's switch to it. + _int_mode = true; + } + + handle_mission_item_both(msg); +} + +void +MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) +{ + + // The mavlink_message could also contain a mavlink_mission_item_int_t. We ignore that here + // and take care of it later in parse_mavlink_mission_item depending on _int_mode. + + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (CHECK_SYSID_COMPID_MISSION(wp)) { + + if (wp.mission_type != _mission_type) { + PX4_WARN("WPM: Unexpected mission type (%u %u)", (int)wp.mission_type, (int)_mission_type); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + return; + } + + if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (wp.seq != _transfer_seq) { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); + + /* request next item again */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + return; + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_transfer_seq == wp.seq + 1) { + // Assume this is a duplicate, where we already successfully got all mission items, + // but the GCS did not receive the last ack and sent the same item again + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer"); + + _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + return; + + } else { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state); + + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + return; + } + + struct mission_item_s mission_item = {}; + + int ret = parse_mavlink_mission_item(&wp, &mission_item); + + if (ret != PX4_OK) { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); + + _mavlink->send_statustext_critical("IGN MISSION_ITEM: Invalid item"); + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); + switch_to_idle_state(); + _transfer_in_progress = false; + return; + } + + bool write_failed = false; + bool check_failed = false; + + switch (_mission_type) { + + case MAV_MISSION_TYPE_MISSION: { + // check that we don't get a wrong item (hardening against wrong client implementations, the list here + // does not need to be complete) + if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) { + check_failed = true; + + } else { + dm_item_t dm_item = _transfer_dataman_id; + + write_failed = dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, + sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); + + if (!write_failed) { + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + } + } + } + break; + + case MAV_MISSION_TYPE_FENCE: { // Write a geofence point + mission_fence_point_s mission_fence_point; + mission_fence_point.nav_cmd = mission_item.nav_cmd; + mission_fence_point.lat = mission_item.lat; + mission_fence_point.lon = mission_item.lon; + mission_fence_point.alt = mission_item.altitude; + + if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + mission_fence_point.vertex_count = mission_item.vertex_count; + + if (mission_item.vertex_count < 3) { // feasibility check + PX4_ERR("Fence: too few vertices"); + check_failed = true; + update_geofence_count(0); + } + + } else { + mission_fence_point.circle_radius = mission_item.circle_radius; + } + + mission_fence_point.frame = mission_item.frame; + + if (!check_failed) { + write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_fence_point, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); + } + + } + break; + + case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point + mission_safe_point_s mission_safe_point; + mission_safe_point.lat = mission_item.lat; + mission_safe_point.lon = mission_item.lon; + mission_safe_point.alt = mission_item.altitude; + mission_safe_point.frame = mission_item.frame; + write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_safe_point, + sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s); + } + break; + + default: + _mavlink->send_statustext_critical("Received unknown mission type, abort."); + break; + } + + if (write_failed || check_failed) { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + + if (write_failed) { + _mavlink->send_statustext_critical("Unable to write on micro SD"); + } + + switch_to_idle_state(); + _transfer_in_progress = false; + return; + } + + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + + PX4_DEBUG("WPM: MISSION_ITEM seq %u received", wp.seq); + + _transfer_seq = wp.seq + 1; + + if (_transfer_seq == _transfer_count) { + /* got all new mission items successfully */ + PX4_DEBUG("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", + _transfer_count, _transfer_current_seq); + + ret = 0; + + switch (_mission_type) { + case MAV_MISSION_TYPE_MISSION: + ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); + break; + + case MAV_MISSION_TYPE_FENCE: + ret = update_geofence_count(_transfer_count); + break; + + case MAV_MISSION_TYPE_RALLY: + ret = update_safepoint_count(_transfer_count); + break; + + default: + PX4_ERR("mission type %u not handled", _mission_type); + break; + } + + // Note: the switch to idle needs to happen after update_geofence_count is called, for proper unlocking order + switch_to_idle_state(); + + + if (ret == PX4_OK) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + _transfer_in_progress = false; + + } else { + /* request next item */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } + } +} + + +void +MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) +{ + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (CHECK_SYSID_COMPID_MISSION(wpca)) { + + if (_state == MAVLINK_WPM_STATE_IDLE) { + /* don't touch mission items storage itself, but only items count in mission state */ + _time_last_recv = hrt_absolute_time(); + + _mission_type = (MAV_MISSION_TYPE)wpca.mission_type; // this is needed for the returned ack + int ret = 0; + + switch (wpca.mission_type) { + case MAV_MISSION_TYPE_MISSION: + ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + break; + + case MAV_MISSION_TYPE_FENCE: + ret = update_geofence_count(0); + break; + + case MAV_MISSION_TYPE_RALLY: + ret = update_safepoint_count(0); + break; + + case MAV_MISSION_TYPE_ALL: + ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + ret = update_geofence_count(0) || ret; + ret = update_safepoint_count(0) || ret; + break; + + default: + PX4_ERR("mission type %u not handled", _mission_type); + break; + } + + if (ret == PX4_OK) { + PX4_DEBUG("WPM: CLEAR_ALL OK (mission_type=%i)", _mission_type); + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + _mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy"); + + PX4_DEBUG("WPM: CLEAR_ALL IGNORED: busy"); + } + } +} + +int +MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, + struct mission_item_s *mission_item) +{ + if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || + mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || + (_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || + mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) { + + // Switch to int mode if that is what we are receiving + if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || + mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) { + _int_mode = true; + } + + if (_int_mode) { + /* The argument is actually a mavlink_mission_item_int_t in int_mode. + * mavlink_mission_item_t and mavlink_mission_item_int_t have the same + * alignment, so we can just swap float for int32_t. */ + const mavlink_mission_item_int_t *item_int + = reinterpret_cast(mavlink_mission_item); + mission_item->lat = ((double)item_int->x) * 1e-7; + mission_item->lon = ((double)item_int->y) * 1e-7; + + } else { + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + } + + mission_item->altitude = mavlink_mission_item->z; + + if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || + mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT) { + mission_item->altitude_is_relative = false; + + } else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || + mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + mission_item->altitude_is_relative = true; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_WAYPOINT: + mission_item->nav_cmd = NAV_CMD_WAYPOINT; + mission_item->time_inside = mavlink_mission_item->param1; + mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + break; + + case MAV_CMD_NAV_LOITER_UNLIM: + mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + mission_item->loiter_radius = mavlink_mission_item->param3; + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + break; + + case MAV_CMD_NAV_LOITER_TIME: + mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + mission_item->time_inside = mavlink_mission_item->param1; + mission_item->force_heading = (mavlink_mission_item->param2 > 0); + mission_item->loiter_radius = mavlink_mission_item->param3; + mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0); + // Yaw is only valid for multicopter but we set it always because + // it's just ignored for fixedwing. + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + break; + + case MAV_CMD_NAV_LAND: + mission_item->nav_cmd = NAV_CMD_LAND; + // TODO: abort alt param1 + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + mission_item->land_precision = mavlink_mission_item->param2; + break; + + case MAV_CMD_NAV_TAKEOFF: + mission_item->nav_cmd = NAV_CMD_TAKEOFF; + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + + break; + + case MAV_CMD_NAV_LOITER_TO_ALT: + mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT; + mission_item->force_heading = (mavlink_mission_item->param1 > 0); + mission_item->loiter_radius = mavlink_mission_item->param2; + mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0); + break; + + case MAV_CMD_NAV_ROI: + case MAV_CMD_DO_SET_ROI: + if ((int)mavlink_mission_item->param1 == MAV_ROI_LOCATION) { + mission_item->nav_cmd = NAV_CMD_DO_SET_ROI; + mission_item->params[0] = MAV_ROI_LOCATION; + + mission_item->params[6] = mavlink_mission_item->z; + + } else if ((int)mavlink_mission_item->param1 == MAV_ROI_NONE) { + mission_item->nav_cmd = NAV_CMD_DO_SET_ROI; + mission_item->params[0] = MAV_ROI_NONE; + + } else { + return MAV_MISSION_INVALID_PARAM1; + } + + break; + + case MAV_CMD_DO_SET_ROI_LOCATION: + mission_item->nav_cmd = NAV_CMD_DO_SET_ROI_LOCATION; + mission_item->params[6] = mavlink_mission_item->z; + break; + + case MAV_CMD_NAV_VTOL_TAKEOFF: + case MAV_CMD_NAV_VTOL_LAND: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + break; + + case MAV_CMD_CONDITION_GATE: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + break; + + case MAV_CMD_NAV_FENCE_RETURN_POINT: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + break; + + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION: + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->vertex_count = (uint16_t)(mavlink_mission_item->param1 + 0.5f); + break; + + case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION: + case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->circle_radius = mavlink_mission_item->param1; + break; + + case MAV_CMD_NAV_RALLY_POINT: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + break; + + default: + mission_item->nav_cmd = NAV_CMD_INVALID; + + PX4_DEBUG("Unsupported command %d", mavlink_mission_item->command); + + return MAV_MISSION_UNSUPPORTED; + } + + mission_item->frame = mavlink_mission_item->frame; + + } else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) { + + // this is a mission item with no coordinates + + mission_item->params[0] = mavlink_mission_item->param1; + mission_item->params[1] = mavlink_mission_item->param2; + mission_item->params[2] = mavlink_mission_item->param3; + mission_item->params[3] = mavlink_mission_item->param4; + + if (_int_mode) { + /* The argument is actually a mavlink_mission_item_int_t in int_mode. + * mavlink_mission_item_t and mavlink_mission_item_int_t have the same + * alignment, so we can just swap float for int32_t. */ + const mavlink_mission_item_int_t *item_int + = reinterpret_cast(mavlink_mission_item); + mission_item->params[4] = ((double)item_int->x); + mission_item->params[5] = ((double)item_int->y); + + } else { + mission_item->params[4] = (double)mavlink_mission_item->x; + mission_item->params[5] = (double)mavlink_mission_item->y; + } + + mission_item->params[6] = mavlink_mission_item->z; + + switch (mavlink_mission_item->command) { + case MAV_CMD_DO_JUMP: + mission_item->nav_cmd = NAV_CMD_DO_JUMP; + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_current_count = 0; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; + + case MAV_CMD_NAV_ROI: + case MAV_CMD_DO_SET_ROI: { + const int roi_mode = mavlink_mission_item->param1; + + if (roi_mode == MAV_ROI_NONE || roi_mode == MAV_ROI_WPNEXT || roi_mode == MAV_ROI_WPINDEX) { + mission_item->nav_cmd = NAV_CMD_DO_SET_ROI; + + } else { + return MAV_MISSION_INVALID_PARAM1; + } + } + break; + + case MAV_CMD_DO_CHANGE_SPEED: + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_SET_SERVO: + case MAV_CMD_DO_LAND_START: + case MAV_CMD_DO_TRIGGER_CONTROL: + case MAV_CMD_DO_DIGICAM_CONTROL: + case MAV_CMD_DO_MOUNT_CONFIGURE: + case MAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_SET_CAMERA_FOCUS: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + case MAV_CMD_IMAGE_START_CAPTURE: + case MAV_CMD_IMAGE_STOP_CAPTURE: + case MAV_CMD_VIDEO_START_CAPTURE: + case MAV_CMD_VIDEO_STOP_CAPTURE: + case MAV_CMD_DO_CONTROL_VIDEO: + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + case MAV_CMD_OBLIQUE_SURVEY: + case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: + case MAV_CMD_SET_CAMERA_MODE: + case MAV_CMD_SET_CAMERA_ZOOM: + case MAV_CMD_DO_VTOL_TRANSITION: + case MAV_CMD_NAV_DELAY: + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: + case MAV_CMD_DO_SET_ROI_NONE: + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + break; + + default: + mission_item->nav_cmd = NAV_CMD_INVALID; + + PX4_DEBUG("Unsupported command %d", mavlink_mission_item->command); + + return MAV_MISSION_UNSUPPORTED; + } + + mission_item->frame = MAV_FRAME_MISSION; + + } else { + PX4_DEBUG("Unsupported frame %d", mavlink_mission_item->frame); + + return MAV_MISSION_UNSUPPORTED_FRAME; + } + + mission_item->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + + mission_item->origin = ORIGIN_MAVLINK; + + return MAV_MISSION_ACCEPTED; +} + + +int +MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, + mavlink_mission_item_t *mavlink_mission_item) +{ + mavlink_mission_item->frame = mission_item->frame; + mavlink_mission_item->command = mission_item->nav_cmd; + mavlink_mission_item->autocontinue = mission_item->autocontinue; + + /* default mappings for generic commands */ + if (mission_item->frame == MAV_FRAME_MISSION) { + mavlink_mission_item->param1 = mission_item->params[0]; + mavlink_mission_item->param2 = mission_item->params[1]; + mavlink_mission_item->param3 = mission_item->params[2]; + mavlink_mission_item->param4 = mission_item->params[3]; + + mavlink_mission_item->x = mission_item->params[4]; + mavlink_mission_item->y = mission_item->params[5]; + + if (_int_mode) { + // This function actually receives a mavlink_mission_item_int_t in _int_mode + // which has the same alignment as mavlink_mission_item_t and the only + // difference is int32_t vs. float for x and y. + mavlink_mission_item_int_t *item_int = + reinterpret_cast(mavlink_mission_item); + + item_int->x = round(mission_item->params[4]); + item_int->y = round(mission_item->params[5]); + + } else { + mavlink_mission_item->x = (float)mission_item->params[4]; + mavlink_mission_item->y = (float)mission_item->params[5]; + } + + mavlink_mission_item->z = mission_item->params[6]; + + switch (mavlink_mission_item->command) { + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + + case NAV_CMD_DO_CHANGE_SPEED: + case NAV_CMD_DO_SET_HOME: + case NAV_CMD_DO_SET_SERVO: + case NAV_CMD_DO_LAND_START: + case NAV_CMD_DO_TRIGGER_CONTROL: + case NAV_CMD_DO_DIGICAM_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + case NAV_CMD_VIDEO_START_CAPTURE: + case NAV_CMD_VIDEO_STOP_CAPTURE: + case NAV_CMD_DO_CONTROL_VIDEO: + case NAV_CMD_DO_MOUNT_CONFIGURE: + case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + case NAV_CMD_DO_SET_ROI: + case NAV_CMD_DO_SET_CAM_TRIGG_DIST: + case NAV_CMD_OBLIQUE_SURVEY: + case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: + case NAV_CMD_SET_CAMERA_MODE: + case NAV_CMD_SET_CAMERA_ZOOM: + case NAV_CMD_SET_CAMERA_FOCUS: + case NAV_CMD_DO_VTOL_TRANSITION: + break; + + default: + return PX4_ERROR; + } + + } else { + mavlink_mission_item->param1 = 0.0f; + mavlink_mission_item->param2 = 0.0f; + mavlink_mission_item->param3 = 0.0f; + mavlink_mission_item->param4 = 0.0f; + + if (_int_mode) { + // This function actually receives a mavlink_mission_item_int_t in _int_mode + // which has the same alignment as mavlink_mission_item_t and the only + // difference is int32_t vs. float for x and y. + mavlink_mission_item_int_t *item_int = + reinterpret_cast(mavlink_mission_item); + + item_int->x = round(mission_item->lat * 1e7); + item_int->y = round(mission_item->lon * 1e7); + + } else { + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + } + + mavlink_mission_item->z = mission_item->altitude; + + if (mission_item->altitude_is_relative) { + if (_int_mode) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + } else { + if (_int_mode) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_INT; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + } + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_WAYPOINT: + mavlink_mission_item->param1 = mission_item->time_inside; + mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param4 = math::degrees(mission_item->yaw); + break; + + case NAV_CMD_LOITER_UNLIMITED: + mavlink_mission_item->param3 = mission_item->loiter_radius; + mavlink_mission_item->param4 = math::degrees(mission_item->yaw); + break; + + case NAV_CMD_LOITER_TIME_LIMIT: + mavlink_mission_item->param1 = mission_item->time_inside; + mavlink_mission_item->param2 = mission_item->force_heading; + mavlink_mission_item->param3 = mission_item->loiter_radius; + mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; + break; + + case NAV_CMD_LAND: + // TODO: param1 abort alt + mavlink_mission_item->param2 = mission_item->land_precision; + mavlink_mission_item->param4 = math::degrees(mission_item->yaw); + break; + + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param4 = math::degrees(mission_item->yaw); + break; + + case NAV_CMD_LOITER_TO_ALT: + mavlink_mission_item->param1 = mission_item->force_heading; + mavlink_mission_item->param2 = mission_item->loiter_radius; + mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; + break; + + case MAV_CMD_NAV_VTOL_TAKEOFF: + case MAV_CMD_NAV_VTOL_LAND: + mavlink_mission_item->param4 = math::degrees(mission_item->yaw); + break; + + case MAV_CMD_NAV_FENCE_RETURN_POINT: + break; + + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION: + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: + mavlink_mission_item->param1 = (float)mission_item->vertex_count; + break; + + case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION: + case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION: + mavlink_mission_item->param1 = mission_item->circle_radius; + break; + + case MAV_CMD_NAV_RALLY_POINT: + break; + + + default: + return PX4_ERROR; + } + } + + return PX4_OK; +} + + +void MavlinkMissionManager::check_active_mission() +{ + // do not send anything over high latency communication + if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { + return; + } + + if (!(_my_dataman_id == _dataman_id)) { + PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating"); + + _my_dataman_id = _dataman_id; + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], + MAV_MISSION_TYPE_MISSION); + } +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_mission.h b/src/prometheus_px4/src/modules/mavlink/mavlink_mission.h new file mode 100644 index 0000000..6740f0c --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_mission.h @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink_mission.h + * Implementation of the MAVLink mission protocol. + * Documentation: + * - http://qgroundcontrol.org/mavlink/mission_interface + * - http://qgroundcontrol.org/mavlink/waypoint_protocol + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include +#include +#include +#include + +#include "mavlink_bridge_header.h" +#include "mavlink_rate_limiter.h" + +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + +static constexpr uint64_t MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT = 5000000; ///< Protocol action timeout in us +static constexpr uint64_t MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT = 250000; ///< Protocol retry timeout in us + +class Mavlink; + +class MavlinkMissionManager +{ +public: + explicit MavlinkMissionManager(Mavlink *mavlink); + + ~MavlinkMissionManager() = default; + + /** + * Handle sending of messages. Call this regularly at a fixed frequency. + * @param t current time + */ + void send(); + + void handle_message(const mavlink_message_t *msg); + + void check_active_mission(void); + +private: + enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state + enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible) + + uint64_t _time_last_recv{0}; + uint64_t _time_last_sent{0}; + + uint8_t _reached_sent_count{0}; ///< last time when the vehicle reached a waypoint + + bool _int_mode{false}; ///< Use accurate int32 instead of float + + unsigned _filesystem_errcount{0}; ///< File system error count + + static dm_item_t _dataman_id; ///< Global Dataman storage ID for active mission + dm_item_t _my_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0}; ///< class Dataman storage ID + + static bool _dataman_init; ///< Dataman initialized + + static uint16_t _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE + static int32_t _current_seq; ///< Current item sequence in active mission + + int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached) + + dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current transmission + + uint16_t _transfer_count{0}; ///< Items count in current transmission + uint16_t _transfer_seq{0}; ///< Item sequence in current transmission + + int32_t _transfer_current_seq{-1}; ///< Current item ID for current transmission (-1 means not initialized) + + uint8_t _transfer_partner_sysid{0}; ///< Partner system ID for current transmission + uint8_t _transfer_partner_compid{0}; ///< Partner component ID for current transmission + + static bool _transfer_in_progress; ///< Global variable checking for current transmission + + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + + uORB::Publication _offboard_mission_pub{ORB_ID(mission)}; + + static uint16_t _geofence_update_counter; + static uint16_t _safepoint_update_counter; + bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress) + + MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz + + Mavlink *_mavlink; + + static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = + 2; ///< Error count limit before stopping to report FS errors + static constexpr uint16_t MAX_COUNT[] = { + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_FENCE_POINTS_MAX - 1, + DM_KEY_SAFE_POINTS_MAX - 1 + }; /**< Maximum number of mission items for each type + (fence & safe points use the first item for the stats) */ + + /** get the maximum number of item count for the current _mission_type */ + uint16_t current_max_item_count(); + + /** get the number of item count for the current _mission_type */ + uint16_t current_item_count(); + + /* do not allow top copying this class */ + MavlinkMissionManager(MavlinkMissionManager &); + MavlinkMissionManager &operator = (const MavlinkMissionManager &); + + void init_offboard_mission(); + + int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); + + /** store the geofence count to dataman */ + int update_geofence_count(unsigned count); + + /** store the safepoint count to dataman */ + int update_safepoint_count(unsigned count); + + /** load geofence stats from dataman */ + int load_geofence_stats(); + + /** load safe point stats from dataman */ + int load_safepoint_stats(); + + /** + * @brief Sends an waypoint ack message + */ + void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type); + + /** + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ + void send_mission_current(int32_t seq); + + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type); + + void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); + + void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq); + + /** + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ + void send_mission_item_reached(uint16_t seq); + + void handle_mission_ack(const mavlink_message_t *msg); + + void handle_mission_set_current(const mavlink_message_t *msg); + + void handle_mission_request_list(const mavlink_message_t *msg); + + void handle_mission_request(const mavlink_message_t *msg); + void handle_mission_request_int(const mavlink_message_t *msg); + void handle_mission_request_both(const mavlink_message_t *msg); + + void handle_mission_count(const mavlink_message_t *msg); + + void handle_mission_item(const mavlink_message_t *msg); + void handle_mission_item_int(const mavlink_message_t *msg); + void handle_mission_item_both(const mavlink_message_t *msg); + + void handle_mission_clear_all(const mavlink_message_t *msg); + + /** + * Parse mavlink MISSION_ITEM message to get mission_item_s. + * + * @param mavlink_mission_item pointer to mavlink_mission_item_t or mavlink_mission_item_int_t + * depending on _int_mode + * @param mission_item pointer to mission_item to construct + */ + int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + + /** + * Format mission_item_s as mavlink MISSION_ITEM(_INT) message. + * + * @param mission_item: pointer to the existing mission item + * @param mavlink_mission_item: pointer to mavlink_mission_item_t or mavlink_mission_item_int_t + * depending on _int_mode. + */ + int format_mavlink_mission_item(const struct mission_item_s *mission_item, + mavlink_mission_item_t *mavlink_mission_item); + + /** + * set _state to idle (and do necessary cleanup) + */ + void switch_to_idle_state(); +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_parameters.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_parameters.cpp new file mode 100644 index 0000000..eb0a31e --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_parameters.cpp @@ -0,0 +1,643 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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 mavlink_parameters.cpp + * Mavlink parameters manager implementation. + * + * @author Anton Babushkin + * @author Lorenz Meier + * @author Beat Kueng + */ + +#include + +#include "mavlink_parameters.h" +#include "mavlink_main.h" +#include + +MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : + _mavlink(mavlink) +{ +} + +unsigned +MavlinkParametersManager::get_size() +{ + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +void +MavlinkParametersManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* request all parameters */ + mavlink_param_request_list_t req_list; + mavlink_msg_param_request_list_decode(msg, &req_list); + + if (req_list.target_system == mavlink_system.sysid && + (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + if (_send_all_index < 0) { + _send_all_index = PARAM_HASH; + + } else { + /* a restart should skip the hash check on the ground */ + _send_all_index = 0; + } + } + + if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && + (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + // publish list request to UAVCAN driver via uORB. + uavcan_parameter_request_s req{}; + req.message_type = msg->msgid; + req.node_id = req_list.target_component; + req.param_index = 0; + req.timestamp = hrt_absolute_time(); + _uavcan_parameter_request_pub.publish(req); + } + + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + /* set parameter */ + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + + /* Whatever the value is, we're being told to stop sending */ + if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) { + + if (_mavlink->hash_check_enabled()) { + _send_all_index = -1; + } + + /* No other action taken, return */ + return; + } + + /* attempt to find parameter, set and send it */ + param_t param = param_find_no_notification(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); + + } else if (!((param_type(param) == PARAM_TYPE_INT32 && set.param_type == MAV_PARAM_TYPE_INT32) || + (param_type(param) == PARAM_TYPE_FLOAT && set.param_type == MAV_PARAM_TYPE_REAL32))) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] param types mismatch param: %s", name); + _mavlink->send_statustext_info(buf); + + } else { + // According to the mavlink spec we should always acknowledge a write operation. + param_set(param, &(set.param_value)); + send_param(param); + } + } + + if (set.target_system == mavlink_system.sysid && set.target_component < 127 && + (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req{}; + req.message_type = msg->msgid; + req.node_id = set.target_component; + req.param_index = -1; + strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + + if (set.param_type == MAV_PARAM_TYPE_REAL32) { + req.param_type = MAV_PARAM_TYPE_REAL32; + req.real_value = set.param_value; + + } else { + int32_t val; + memcpy(&val, &set.param_value, sizeof(int32_t)); + req.param_type = MAV_PARAM_TYPE_INT64; + req.int_value = val; + } + + req.timestamp = hrt_absolute_time(); + _uavcan_parameter_request_pub.publish(req); + } + + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + /* request one parameter */ + mavlink_param_request_read_t req_read; + mavlink_msg_param_request_read_decode(msg, &req_read); + + if (req_read.target_system == mavlink_system.sysid && + (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + + /* when no index is given, loop through string ids and compare them */ + if (req_read.param_index < 0) { + /* XXX: I left this in so older versions of QGC wouldn't break */ + if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) { + /* return hash check for cached params */ + uint32_t hash = param_hash_check(); + + /* build the one-off response message */ + mavlink_param_value_t param_value; + param_value.param_count = param_count_used(); + param_value.param_index = -1; + strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + param_value.param_type = MAV_PARAM_TYPE_UINT32; + memcpy(¶m_value.param_value, &hash, sizeof(hash)); + mavlink_msg_param_value_send_struct(_mavlink->get_channel(), ¶m_value); + + } else { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + send_param(param_find_no_notification(name)); + } + + } else { + /* when index is >= 0, send this parameter again */ + int ret = send_param(param_for_used_index(req_read.param_index)); + + if (ret == 1) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + + } else if (ret == 2) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } + } + } + + if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && + (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req{}; + req.timestamp = hrt_absolute_time(); + req.message_type = msg->msgid; + req.node_id = req_read.target_component; + req.param_index = req_read.param_index; + strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + + // Enque the request and forward the first to the uavcan node + enque_uavcan_request(&req); + request_next_uavcan_parameter(); + } + + break; + } + + case MAVLINK_MSG_ID_PARAM_MAP_RC: { + /* map a rc channel to a parameter */ + mavlink_param_map_rc_t map_rc; + mavlink_msg_param_map_rc_decode(msg, &map_rc); + + if (map_rc.target_system == mavlink_system.sysid && + (map_rc.target_component == mavlink_system.compid || + map_rc.target_component == MAV_COMP_ID_ALL)) { + + /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ + size_t i = map_rc.parameter_rc_channel_index; + + if (i >= sizeof(_rc_param_map.param_index) / sizeof(_rc_param_map.param_index[0])) { + mavlink_log_warning(_mavlink->get_mavlink_log_pub(), "parameter_rc_channel_index out of bounds"); + break; + } + + _rc_param_map.param_index[i] = map_rc.param_index; + strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, + MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + _rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0'; + _rc_param_map.scale[i] = map_rc.scale; + _rc_param_map.value0[i] = map_rc.param_value0; + _rc_param_map.value_min[i] = map_rc.param_value_min; + _rc_param_map.value_max[i] = map_rc.param_value_max; + + if (map_rc.param_index == -2) { // -2 means unset map + _rc_param_map.valid[i] = false; + + } else { + _rc_param_map.valid[i] = true; + } + + _rc_param_map.timestamp = hrt_absolute_time(); + _rc_param_map_pub.publish(_rc_param_map); + } + + break; + } + + default: + break; + } +} + +void +MavlinkParametersManager::send() +{ + if (!_first_send) { + // parameters QGC can't tolerate not finding (2020-11-11) + param_find("BAT_A_PER_V"); + param_find("BAT_CRIT_THR"); + param_find("BAT_EMERGEN_THR"); + param_find("BAT_LOW_THR"); + param_find("BAT_N_CELLS"); + param_find("BAT_V_CHARGED"); + param_find("BAT_V_DIV"); + param_find("BAT_V_EMPTY"); + param_find("CAL_ACC0_ID"); + param_find("CAL_GYRO0_ID"); + param_find("CAL_MAG0_ID"); + param_find("CAL_MAG0_ROT"); + param_find("CAL_MAG1_ID"); + param_find("CAL_MAG1_ROT"); + param_find("CAL_MAG2_ID"); + param_find("CAL_MAG2_ROT"); + param_find("CAL_MAG3_ID"); + param_find("CAL_MAG3_ROT"); + param_find("SENS_BOARD_ROT"); + param_find("SENS_BOARD_X_OFF"); + param_find("SENS_BOARD_Y_OFF"); + param_find("SENS_BOARD_Z_OFF"); + param_find("SENS_DPRES_OFF"); + param_find("TRIG_MODE"); + param_find("UAVCAN_ENABLE"); + + _first_send = true; + } + + int max_num_to_send; + + if (_mavlink->get_protocol() == Protocol::SERIAL && !_mavlink->is_usb_uart()) { + max_num_to_send = 3; + + } else { + // speed up parameter loading via UDP or USB: try to send 20 at once + max_num_to_send = 20; + } + + int i = 0; + + // Send while burst is not exceeded, we still have buffer space and still something to send + while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() + && send_params()) {} +} + +bool +MavlinkParametersManager::send_params() +{ + if (send_uavcan()) { + return true; + + } else if (send_one()) { + return true; + + } else if (send_untransmitted()) { + return true; + + } else { + return false; + } +} + +bool +MavlinkParametersManager::send_untransmitted() +{ + bool sent_one = false; + + if (_parameter_update_sub.updated()) { + // clear the update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // Schedule an update if not already the case + if (_param_update_time == 0) { + _param_update_time = pupdate.timestamp; + _param_update_index = 0; + } + } + + if ((_param_update_time != 0) && ((_param_update_time + 5 * 1000) < hrt_absolute_time())) { + + param_t param = 0; + + // send out all changed values + do { + // skip over all parameters which are not invalid and not used + do { + param = param_for_index(_param_update_index); + ++_param_update_index; + } while (param != PARAM_INVALID && !param_used(param)); + + // send parameters which are untransmitted while there is + // space in the TX buffer + if ((param != PARAM_INVALID) && param_value_unsaved(param)) { + int ret = send_param(param); + char buf[100]; + strncpy(&buf[0], param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + sent_one = true; + + if (ret != PX4_OK) { + break; + } + } + } while ((_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() + && (_param_update_index < (int) param_count())); + + // Flag work as done once all params have been sent + if (_param_update_index >= (int) param_count()) { + _param_update_time = 0; + } + } + + return sent_one; +} + +bool +MavlinkParametersManager::send_uavcan() +{ + /* Send parameter values received from the UAVCAN topic */ + uavcan_parameter_value_s value{}; + + if (_uavcan_parameter_value_sub.update(&value)) { + + // Check if we received a matching parameter, drop it from the list and request the next + if ((_uavcan_open_request_list != nullptr) + && (value.param_index == _uavcan_open_request_list->req.param_index) + && (value.node_id == _uavcan_open_request_list->req.node_id)) { + + dequeue_uavcan_request(); + request_next_uavcan_parameter(); + } + + mavlink_param_value_t msg{}; + msg.param_count = value.param_count; + msg.param_index = value.param_index; +#if defined(__GNUC__) && __GNUC__ >= 8 +#pragma GCC diagnostic ignored "-Wstringop-truncation" +#endif + /* + * coverity[buffer_size_warning : FALSE] + * + * The MAVLink spec does not require the string to be NUL-terminated if it + * has length 16. In this case the receiving end needs to terminate it + * when copying it. + */ + strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); +#if defined(__GNUC__) && __GNUC__ >= 8 +#pragma GCC diagnostic pop +#endif + + if (value.param_type == MAV_PARAM_TYPE_REAL32) { + msg.param_type = MAVLINK_TYPE_FLOAT; + msg.param_value = value.real_value; + + } else { + int32_t val = (int32_t)value.int_value; + memcpy(&msg.param_value, &val, sizeof(int32_t)); + msg.param_type = MAVLINK_TYPE_INT32_T; + } + + // Re-pack the message with the UAVCAN node ID + mavlink_message_t mavlink_packet{}; + mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet, + &msg); + _mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet); + + return true; + } + + return false; +} + +bool +MavlinkParametersManager::send_one() +{ + if (_send_all_index >= 0) { + /* send all parameters if requested, but only after the system has booted */ + + /* The first thing we send is a hash of all values for the ground + * station to try and quickly load a cached copy of our params + */ + if (_send_all_index == PARAM_HASH) { + /* return hash check for cached params */ + uint32_t hash = param_hash_check(); + + /* build the one-off response message */ + mavlink_param_value_t msg; + msg.param_count = param_count_used(); + msg.param_index = -1; + strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + msg.param_type = MAV_PARAM_TYPE_UINT32; + memcpy(&msg.param_value, &hash, sizeof(hash)); + mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); + + /* after this we should start sending all params */ + _send_all_index = 0; + + /* No further action, return now */ + return true; + } + + /* look for the first parameter which is used */ + param_t p; + + do { + /* walk through all parameters, including unused ones */ + p = param_for_index(_send_all_index); + _send_all_index++; + } while (p != PARAM_INVALID && !param_used(p)); + + if (p != PARAM_INVALID) { + send_param(p); + } + + if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { + _send_all_index = -1; + return false; + + } else { + return true; + } + } + + return false; +} + +int +MavlinkParametersManager::send_param(param_t param, int component_id) +{ + if (param == PARAM_INVALID) { + return 1; + } + + /* no free TX buf to send this param */ + if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) { + return 1; + } + + mavlink_param_value_t msg; + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + float param_value{}; + + if (param_get(param, ¶m_value) != OK) { + return 2; + } + + msg.param_value = param_value; + + msg.param_count = param_count_used(); + msg.param_index = param_get_used_index(param); + +#if defined(__GNUC__) && __GNUC__ >= 8 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstringop-truncation" +#endif + /* + * coverity[buffer_size_warning : FALSE] + * + * The MAVLink spec does not require the string to be NUL-terminated if it + * has length 16. In this case the receiving end needs to terminate it + * when copying it. + */ + strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); +#if defined(__GNUC__) && __GNUC__ >= 8 +#pragma GCC diagnostic pop +#endif + + /* query parameter type */ + param_type_t type = param_type(param); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + if (type == PARAM_TYPE_INT32) { + msg.param_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + msg.param_type = MAVLINK_TYPE_FLOAT; + + } else { + msg.param_type = MAVLINK_TYPE_FLOAT; + } + + /* default component ID */ + if (component_id < 0) { + mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); + + } else { + // Re-pack the message with a different component ID + mavlink_message_t mavlink_packet; + mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink->get_channel(), &mavlink_packet, &msg); + _mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet); + } + + return 0; +} + +void MavlinkParametersManager::request_next_uavcan_parameter() +{ + // Request a parameter if we are not already waiting on a response and if the list is not empty + if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) { + uavcan_parameter_request_s req = _uavcan_open_request_list->req; + + _uavcan_parameter_request_pub.publish(req); + + _uavcan_waiting_for_request_response = true; + } +} + +void MavlinkParametersManager::enque_uavcan_request(uavcan_parameter_request_s *req) +{ + // We store at max 10 requests to keep memory consumption low. + // Dropped requests will be repeated by the ground station + if (_uavcan_queued_request_items >= 10) { + return; + } + + _uavcan_open_request_list_item *new_reqest = new _uavcan_open_request_list_item; + new_reqest->req = *req; + new_reqest->next = nullptr; + + _uavcan_open_request_list_item *item = _uavcan_open_request_list; + ++_uavcan_queued_request_items; + + if (item == nullptr) { + // Add the first item to the list + _uavcan_open_request_list = new_reqest; + + } else { + // Find the last item and add the new request at the end + while (item->next != nullptr) { + item = item->next; + } + + item->next = new_reqest; + } +} + +void MavlinkParametersManager::dequeue_uavcan_request() +{ + if (_uavcan_open_request_list != nullptr) { + // Drop the first item in the list and free the used memory + _uavcan_open_request_list_item *first = _uavcan_open_request_list; + _uavcan_open_request_list = first->next; + --_uavcan_queued_request_items; + delete first; + _uavcan_waiting_for_request_response = false; + } +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_parameters.h b/src/prometheus_px4/src/modules/mavlink/mavlink_parameters.h new file mode 100644 index 0000000..d55ed8e --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_parameters.h @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink_parameters.h + * Mavlink parameters manager definition. + * + * @author Anton Babushkin + * @author Lorenz Meier + * @author Beat Kueng + */ + +#pragma once + +#include + +#include "mavlink_bridge_header.h" +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class Mavlink; + +class MavlinkParametersManager +{ +public: + explicit MavlinkParametersManager(Mavlink *mavlink); + ~MavlinkParametersManager() = default; + + /** + * Handle sending of messages. Call this regularly at a fixed frequency. + * @param t current time + */ + void send(); + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + +private: + int _send_all_index{-1}; + + /* do not allow top copying this class */ + MavlinkParametersManager(MavlinkParametersManager &); + MavlinkParametersManager &operator = (const MavlinkParametersManager &); + +protected: + /// send a single param if a PARAM_REQUEST_LIST is in progress + /// @return true if a parameter was sent + bool send_one(); + + /** + * Handle any open param send transfer + */ + bool send_params(); + + /** + * Send UAVCAN params + */ + bool send_uavcan(); + + /** + * Send untransmitted params + */ + bool send_untransmitted(); + + int send_param(param_t param, int component_id = -1); + + // Item of a single-linked list to store requested uavcan parameters + struct _uavcan_open_request_list_item { + uavcan_parameter_request_s req; + struct _uavcan_open_request_list_item *next; + }; + + /** + * Request the next uavcan parameter + */ + void request_next_uavcan_parameter(); + + /** + * Enqueue one uavcan parameter reqest. We store 10 at max. + */ + void enque_uavcan_request(uavcan_parameter_request_s *req); + + /** + * Drop the first reqest from the list + */ + void dequeue_uavcan_request(); + + _uavcan_open_request_list_item *_uavcan_open_request_list{nullptr}; ///< Pointer to the first item in the linked list + bool _uavcan_waiting_for_request_response{false}; ///< We have reqested a parameter and wait for the response + uint16_t _uavcan_queued_request_items{0}; ///< Number of stored parameter requests currently in the list + + uORB::Publication _rc_param_map_pub{ORB_ID(rc_parameter_map)}; + rc_parameter_map_s _rc_param_map{}; + + uORB::Publication _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)}; + // enforce ORB_ID(uavcan_parameter_request) constants that map to MAVLINK defines + static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ == MAVLINK_MSG_ID_PARAM_REQUEST_READ, + "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_READ constant mismatch"); + static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET == MAVLINK_MSG_ID_PARAM_SET, + "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_SET constant mismatch"); + static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST == MAVLINK_MSG_ID_PARAM_REQUEST_LIST, + "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_LIST constant mismatch"); + static_assert(uavcan_parameter_request_s::NODE_ID_ALL == MAV_COMP_ID_ALL, + "uavcan_parameter_request_s MAV_COMP_ID_ALL constant mismatch"); + static_assert(uavcan_parameter_request_s::PARAM_TYPE_UINT8 == MAV_PARAM_TYPE_UINT8, + "uavcan_parameter_request_s MAV_PARAM_TYPE_UINT8 constant mismatch"); + static_assert(uavcan_parameter_request_s::PARAM_TYPE_REAL32 == MAV_PARAM_TYPE_REAL32, + "uavcan_parameter_request_s MAV_PARAM_TYPE_REAL32 constant mismatch"); + static_assert(uavcan_parameter_request_s::PARAM_TYPE_INT64 == MAV_PARAM_TYPE_INT64, + "uavcan_parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch"); + + uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + hrt_abstime _param_update_time{0}; + int _param_update_index{0}; + + Mavlink *_mavlink; + + bool _first_send{false}; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_params.c b/src/prometheus_px4/src/modules/mavlink/mavlink_params.c new file mode 100644 index 0000000..fb88876 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_params.c @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * MAVLink system ID + * @group MAVLink + * @min 1 + * @max 250 + * @reboot_required true + */ +PARAM_DEFINE_INT32(MAV_SYS_ID, 1); + +/** + * MAVLink component ID + * @group MAVLink + * @min 1 + * @max 250 + * @reboot_required true + */ +PARAM_DEFINE_INT32(MAV_COMP_ID, 1); + +/** + * MAVLink protocol version + * @group MAVLink + * @value 0 Default to 1, switch to 2 if GCS sends version 2 + * @value 1 Always use version 1 + * @value 2 Always use version 2 + */ +PARAM_DEFINE_INT32(MAV_PROTO_VER, 0); + +/** + * MAVLink SiK Radio ID + * + * When non-zero the MAVLink app will attempt to configure the + * SiK radio to this ID and re-set the parameter to 0. If the value + * is negative it will reset the complete radio config to + * factory defaults. Only applies if this mavlink instance is going through a SiK radio + * + * @group MAVLink + * @min -1 + * @max 240 + */ +PARAM_DEFINE_INT32(MAV_SIK_RADIO_ID, 0); + +/** + * MAVLink airframe type + * + * @min 1 + * @max 27 + * @value 0 Generic micro air vehicle + * @value 1 Fixed wing aircraft + * @value 2 Quadrotor + * @value 3 Coaxial helicopter + * @value 4 Normal helicopter with tail rotor + * @value 5 Ground installation + * @value 6 Operator control unit / ground control station + * @value 7 Airship, controlled + * @value 8 Free balloon, uncontrolled + * @value 9 Rocket + * @value 10 Ground rover + * @value 11 Surface vessel, boat, ship + * @value 12 Submarine + * @value 13 Hexarotor + * @value 14 Octorotor + * @value 15 Tricopter + * @value 16 Flapping wing + * @value 17 Kite + * @value 18 Onboard companion controller + * @value 19 Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. + * @value 20 Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. + * @value 21 Tiltrotor VTOL + * @value 22 VTOL reserved 2 + * @value 23 VTOL reserved 3 + * @value 24 VTOL reserved 4 + * @value 25 VTOL reserved 5 + * @value 26 Onboard gimbal + * @value 27 Onboard ADSB peripheral + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_TYPE, 2); + +/** + * Use/Accept HIL GPS message even if not in HIL mode + * + * If set to 1 incoming HIL GPS messages are parsed. + * + * @boolean + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); + +/** + * Forward external setpoint messages + * + * If set to 1 incoming external setpoint messages will be directly forwarded + * to the controllers if in offboard control mode + * + * @boolean + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); + +/** + * Parameter hash check. + * + * Disabling the parameter hash check functionality will make the mavlink instance + * stream parameters continuously. + * + * @boolean + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1); + +/** + * Hearbeat message forwarding. + * + * The mavlink hearbeat message will not be forwarded if this parameter is set to 'disabled'. + * The main reason for disabling heartbeats to be forwarded is because they confuse dronekit. + * + * @boolean + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1); + +/** + * Activate ODOMETRY loopback. + * + * If set, it gets the data from 'vehicle_visual_odometry' instead of 'vehicle_odometry' + * serving as a loopback of the received ODOMETRY messages on the Mavlink receiver. + * + * @boolean + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_ODOM_LP, 0); + +/** + * Timeout in seconds for the RADIO_STATUS reports coming in + * + * If the connected radio stops reporting RADIO_STATUS for a certain time, + * a warning is triggered and, if MAV_X_RADIO_CTL is enabled, the software-flow + * control is reset. + * + * @group MAVLink + * @unit s + * @min 1 + * @max 250 + */ +PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5); diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_rate_limiter.cpp new file mode 100644 index 0000000..9c69b5e --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 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 mavlink_rate_limiter.cpp + * Message rate limiter implementation. + * + * @author Anton Babushkin + */ + +#include "mavlink_rate_limiter.h" + +bool +MavlinkRateLimiter::check(const hrt_abstime &t) +{ + uint64_t dt = t - _last_sent; + + if (dt > 0 && dt >= _interval) { + _last_sent = t; + return true; + } + + return false; +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_rate_limiter.h b/src/prometheus_px4/src/modules/mavlink/mavlink_rate_limiter.h new file mode 100644 index 0000000..39d86bb --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_rate_limiter.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 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 mavlink_rate_limiter.h + * Message rate limiter definition. + * + * @author Anton Babushkin + */ + +#ifndef MAVLINK_RATE_LIMITER_H_ +#define MAVLINK_RATE_LIMITER_H_ + +#include + + +class MavlinkRateLimiter +{ +private: + hrt_abstime _last_sent{0}; + hrt_abstime _interval{1000000}; + +public: + MavlinkRateLimiter() = default; + MavlinkRateLimiter(unsigned int interval) : _interval(interval) {}; + + ~MavlinkRateLimiter() = default; + + void set_interval(unsigned int interval) { _interval = interval; } + + bool check(const hrt_abstime &t); +}; + + +#endif /* MAVLINK_RATE_LIMITER_H_ */ diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_receiver.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_receiver.cpp new file mode 100644 index 0000000..a6300c9 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_receiver.cpp @@ -0,0 +1,3235 @@ +/**************************************************************************** + * + * 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 mavlink_receiver.cpp + * MAVLink protocol message receive and dispatch + * + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#include +#include +#include +#include +#include + +#include +#include + +#ifdef CONFIG_NET +#include +#include +#include +#endif + +#ifndef __PX4_POSIX +#include +#endif + +#include "mavlink_command_sender.h" +#include "mavlink_main.h" +#include "mavlink_receiver.h" + +#include // For DeviceId union + +#ifdef CONFIG_NET +#define MAVLINK_RECEIVER_NET_ADDED_STACK 1360 +#else +#define MAVLINK_RECEIVER_NET_ADDED_STACK 0 +#endif + +MavlinkReceiver::~MavlinkReceiver() +{ + delete _tune_publisher; + delete _px4_accel; + delete _px4_baro; + delete _px4_gyro; + delete _px4_mag; +} + +MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : + ModuleParams(nullptr), + _mavlink(parent), + _mavlink_ftp(parent), + _mavlink_log_handler(parent), + _mission_manager(parent), + _parameters_manager(parent), + _mavlink_timesync(parent) +{ +} + +void +MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result) +{ + vehicle_command_ack_s command_ack{}; + + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = command; + command_ack.result = result; + command_ack.target_system = sysid; + command_ack.target_component = compid; + + _cmd_ack_pub.publish(command_ack); +} + +void +MavlinkReceiver::handle_message(mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_COMMAND_LONG: + handle_message_command_long(msg); + break; + + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + + case MAVLINK_MSG_ID_COMMAND_ACK: + handle_message_command_ack(msg); + break; + + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_message_optical_flow_rad(msg); + break; + + case MAVLINK_MSG_ID_PING: + handle_message_ping(msg); + break; + + case MAVLINK_MSG_ID_SET_MODE: + handle_message_set_mode(msg); + break; + + case MAVLINK_MSG_ID_ATT_POS_MOCAP: + handle_message_att_pos_mocap(msg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_message_set_position_target_local_ned(msg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: + handle_message_set_position_target_global_int(msg); + break; + + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_message_set_attitude_target(msg); + break; + + case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET: + handle_message_set_actuator_control_target(msg); + break; + + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); + break; + + case MAVLINK_MSG_ID_ODOMETRY: + handle_message_odometry(msg); + break; + + case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN: + handle_message_set_gps_global_origin(msg); + break; + + case MAVLINK_MSG_ID_RADIO_STATUS: + handle_message_radio_status(msg); + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + handle_message_manual_control(msg); + break; + + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + handle_message_rc_channels_override(msg); + break; + + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(msg); + break; + + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + handle_message_distance_sensor(msg); + break; + + case MAVLINK_MSG_ID_FOLLOW_TARGET: + handle_message_follow_target(msg); + break; + + case MAVLINK_MSG_ID_LANDING_TARGET: + handle_message_landing_target(msg); + break; + + case MAVLINK_MSG_ID_CELLULAR_STATUS: + handle_message_cellular_status(msg); + break; + + case MAVLINK_MSG_ID_ADSB_VEHICLE: + handle_message_adsb_vehicle(msg); + break; + + case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION: + handle_message_utm_global_position(msg); + break; + + case MAVLINK_MSG_ID_COLLISION: + handle_message_collision(msg); + break; + + case MAVLINK_MSG_ID_GPS_RTCM_DATA: + handle_message_gps_rtcm_data(msg); + break; + + case MAVLINK_MSG_ID_BATTERY_STATUS: + handle_message_battery_status(msg); + break; + + case MAVLINK_MSG_ID_SERIAL_CONTROL: + handle_message_serial_control(msg); + break; + + case MAVLINK_MSG_ID_LOGGING_ACK: + handle_message_logging_ack(msg); + break; + + case MAVLINK_MSG_ID_PLAY_TUNE: + handle_message_play_tune(msg); + break; + + case MAVLINK_MSG_ID_PLAY_TUNE_V2: + handle_message_play_tune_v2(msg); + break; + + case MAVLINK_MSG_ID_OBSTACLE_DISTANCE: + handle_message_obstacle_distance(msg); + break; + + case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER: + handle_message_trajectory_representation_bezier(msg); + break; + + case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS: + handle_message_trajectory_representation_waypoints(msg); + break; + + case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS: + handle_message_onboard_computer_status(msg); + break; + + case MAVLINK_MSG_ID_GENERATOR_STATUS: + handle_message_generator_status(msg); + break; + + case MAVLINK_MSG_ID_STATUSTEXT: + handle_message_statustext(msg); + break; + +#if !defined(CONSTRAINED_FLASH) + + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + handle_message_named_value_float(msg); + break; + + case MAVLINK_MSG_ID_DEBUG: + handle_message_debug(msg); + break; + + case MAVLINK_MSG_ID_DEBUG_VECT: + handle_message_debug_vect(msg); + break; + + case MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY: + handle_message_debug_float_array(msg); + break; +#endif // !CONSTRAINED_FLASH + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: + handle_message_gimbal_manager_set_attitude(msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL: + handle_message_gimbal_manager_set_manual_control(msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: + handle_message_gimbal_device_information(msg); + break; + + default: + break; + } + + /* + * Only decode hil messages in HIL mode. + * + * The HIL mode is enabled by the HIL bit flag + * in the system mode. Either send a set mode + * COMMAND_LONG message or a SET_MODE message + * + * Accept HIL GPS messages if use_hil_gps flag is true. + * This allows to provide fake gps measurements to the system. + */ + if (_mavlink->get_hil_enabled()) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor(msg); + break; + + case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: + handle_message_hil_state_quaternion(msg); + break; + + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_hil_optical_flow(msg); + break; + + default: + break; + } + } + + + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; + + default: + break; + } + + } + + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); +} + +bool +MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component) +{ + /* evaluate if this system should accept this command */ + bool target_ok = false; + + switch (command) { + + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: + case MAV_CMD_REQUEST_PROTOCOL_VERSION: + /* broadcast and ignore component */ + target_ok = (target_system == 0) || (target_system == mavlink_system.sysid); + break; + + default: + target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid) + || (target_component == MAV_COMP_ID_ALL)); + break; + } + + return target_ok; +} + +void +MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + vehicle_command_s vcmd{}; + + vcmd.timestamp = hrt_absolute_time(); + + const float before_int32_max = nextafterf((float)INT32_MAX, 0.0f); + const float after_int32_max = nextafterf((float)INT32_MAX, (float)INFINITY); + + if (cmd_mavlink.param5 >= before_int32_max && cmd_mavlink.param5 <= after_int32_max && + cmd_mavlink.param6 >= before_int32_max && cmd_mavlink.param6 <= after_int32_max) { + // This looks suspicously like INT32_MAX was sent in a COMMAND_LONG instead of + // a COMMAND_INT. + PX4_ERR("param5/param6 invalid of command %" PRIu16, cmd_mavlink.command); + acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_DENIED); + return; + } + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = (double)cmd_mavlink.param5; + vcmd.param6 = (double)cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + vcmd.from_external = true; + + handle_message_command_both(msg, cmd_mavlink, vcmd); +} + +void +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); + + if (cmd_mavlink.x == 0x7ff80000 && cmd_mavlink.y == 0x7ff80000) { + // This looks like NAN was by accident sent as int. + PX4_ERR("x/y invalid of command %" PRIu16, cmd_mavlink.command); + acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_DENIED); + return; + } + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + + if (cmd_mavlink.x == INT32_MAX && cmd_mavlink.y == INT32_MAX) { + // INT32_MAX for x and y means to ignore it. + vcmd.param5 = (double)NAN; + vcmd.param6 = (double)NAN; + + } else { + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + } + + vcmd.param7 = cmd_mavlink.z; + vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = false; + vcmd.from_external = true; + + handle_message_command_both(msg, cmd_mavlink, vcmd); +} + +template +void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink, + const vehicle_command_s &vehicle_command) +{ + bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component); + bool send_ack = true; + uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + + if (!target_ok) { + acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED); + return; + } + + // First we handle legacy support requests which were used before we had + // the generic MAV_CMD_REQUEST_MESSAGE. + if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { + result = handle_request_message_command(MAVLINK_MSG_ID_AUTOPILOT_VERSION); + + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) { + result = handle_request_message_command(MAVLINK_MSG_ID_PROTOCOL_VERSION); + + } else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) { + result = handle_request_message_command(MAVLINK_MSG_ID_HOME_POSITION); + + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) { + result = handle_request_message_command(MAVLINK_MSG_ID_FLIGHT_INFORMATION); + + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_STORAGE_INFORMATION) { + result = handle_request_message_command(MAVLINK_MSG_ID_STORAGE_INFORMATION); + + } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { + if (set_message_interval((int)roundf(cmd_mavlink.param1), cmd_mavlink.param2, cmd_mavlink.param3)) { + result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + } + + } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { + get_message_interval((int)roundf(cmd_mavlink.param1)); + + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { + + uint16_t message_id = (uint16_t)roundf(vehicle_command.param1); + result = handle_request_message_command(message_id, + vehicle_command.param2, vehicle_command.param3, vehicle_command.param4, + vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); + + } else if (cmd_mavlink.command == MAV_CMD_SET_CAMERA_ZOOM) { + struct actuator_controls_s actuator_controls = {}; + actuator_controls.timestamp = hrt_absolute_time(); + + for (size_t i = 0; i < 8; i++) { + actuator_controls.control[i] = NAN; + } + + switch ((int)(cmd_mavlink.param1 + 0.5f)) { + case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_RANGE: + actuator_controls.control[actuator_controls_s::INDEX_CAMERA_ZOOM] = cmd_mavlink.param2 / 50.0f - 1.0f; + break; + + case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_STEP: + case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS: + case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH: + default: + send_ack = false; + } + + _actuator_controls_pubs[actuator_controls_s::GROUP_INDEX_GIMBAL].publish(actuator_controls); + + } else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) { + if (_mavlink->failure_injection_enabled()) { + _cmd_pub.publish(vehicle_command); + send_ack = false; + + } else { + result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; + send_ack = true; + } + + } else if (cmd_mavlink.command == MAV_CMD_DO_SET_ACTUATOR) { + // since we're only paying attention to 3 AUX outputs, the + // index should be 0, otherwise ignore the message + if (((int) vehicle_command.param7) == 0) { + actuator_controls_s actuator_controls{}; + // update with existing values to avoid changing unspecified controls + _actuator_controls_3_sub.update(&actuator_controls); + + actuator_controls.timestamp = hrt_absolute_time(); + + bool updated = false; + + if (PX4_ISFINITE(vehicle_command.param1)) { + actuator_controls.control[5] = vehicle_command.param1; + updated = true; + } + + if (PX4_ISFINITE(vehicle_command.param2)) { + actuator_controls.control[6] = vehicle_command.param2; + updated = true; + } + + if (PX4_ISFINITE(vehicle_command.param3)) { + actuator_controls.control[7] = vehicle_command.param3; + updated = true; + } + + if (updated) { + _actuator_controls_pubs[3].publish(actuator_controls); + } + } + + } else { + send_ack = false; + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + PX4_WARN("ignoring CMD with same SYS/COMP (%" PRIu8 "/%" PRIu8 ") ID", mavlink_system.sysid, mavlink_system.compid); + return; + } + + if (cmd_mavlink.command == MAV_CMD_LOGGING_START) { + // check that we have enough bandwidth available: this is given by the configured logger topics + // and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming + // on a radio link + if (_mavlink->get_data_rate() < 5000) { + send_ack = true; + result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; + _mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming"); + + } else { + // we already instanciate the streaming object, because at this point we know on which + // mavlink channel streaming was requested. But in fact it's possible that the logger is + // not even running. The main mavlink thread takes care of this by waiting for an ack + // from the logger. + _mavlink->try_start_ulog_streaming(msg->sysid, msg->compid); + } + + } else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) { + _mavlink->request_stop_ulog_streaming(); + } + + if (!send_ack) { + _cmd_pub.publish(vehicle_command); + } + } + + if (send_ack) { + acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result); + } +} + +uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, float param2, float param3, float param4, + float param5, float param6, float param7) +{ + bool stream_found = false; + bool message_sent = false; + + for (const auto &stream : _mavlink->get_streams()) { + if (stream->get_id() == message_id) { + stream_found = true; + message_sent = stream->request_message(param2, param3, param4, param5, param6, param7); + break; + } + } + + if (!stream_found) { + // If we don't find the stream, we can configure it with rate 0 and then trigger it once. + const char *stream_name = get_stream_name(message_id); + + if (stream_name != nullptr) { + _mavlink->configure_stream_threadsafe(stream_name, 0.0f); + + // Now we try again to send it. + for (const auto &stream : _mavlink->get_streams()) { + if (stream->get_id() == message_id) { + message_sent = stream->request_message(param2, param3, param4, param5, param6, param7); + break; + } + } + } + } + + return (message_sent ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED); +} + + +void +MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) +{ + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(msg, &ack); + + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink->get_channel()); + + vehicle_command_ack_s command_ack{}; + + command_ack.timestamp = hrt_absolute_time(); + command_ack.result_param2 = ack.result_param2; + command_ack.command = ack.command; + command_ack.result = ack.result; + command_ack.from_external = true; + command_ack.result_param1 = ack.progress; + command_ack.target_system = ack.target_system; + command_ack.target_component = ack.target_component; + + _cmd_ack_pub.publish(command_ack); + + // TODO: move it to the same place that sent the command + if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) { + if (msg->compid == MAV_COMP_ID_CAMERA) { + PX4_WARN("Got unsuccessful result %" PRIu8 " from camera", ack.result); + } + } +} + +void +MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_optical_flow_rad_t flow; + mavlink_msg_optical_flow_rad_decode(msg, &flow); + + optical_flow_s f{}; + + f.timestamp = hrt_absolute_time(); + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.gyro_temperature = flow.temperature; + f.ground_distance_m = flow.distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + f.max_flow_rate = _param_sens_flow_maxr.get(); + f.min_ground_distance = _param_sens_flow_minhgt.get(); + f.max_ground_distance = _param_sens_flow_maxhgt.get(); + + /* read flow sensor parameters */ + const Rotation flow_rot = (Rotation)_param_sens_flow_rot.get(); + + /* rotate measurements according to parameter */ + float zero_val = 0.0f; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val); + rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral); + + _flow_pub.publish(f); + + /* Use distance value for distance sensor topic */ + if (flow.distance > 0.0f) { // negative values signal invalid data + + distance_sensor_s d{}; + + device::Device::DeviceId device_id; + device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; + device_id.devid_s.address = msg->sysid; + + d.timestamp = f.timestamp; + d.min_distance = 0.3f; + d.max_distance = 5.0f; + d.current_distance = flow.distance; /* both are in m */ + d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + d.device_id = device_id.devid; + d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + d.variance = 0.0; + + _flow_distance_sensor_pub.publish(d); + } +} + +void +MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + + optical_flow_s f{}; + + f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; + + _flow_pub.publish(f); + + /* Use distance value for distance sensor topic */ + distance_sensor_s d{}; + + device::Device::DeviceId device_id; + device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; + device_id.devid_s.address = msg->sysid; + + d.timestamp = hrt_absolute_time(); + d.min_distance = 0.3f; + d.max_distance = 5.0f; + d.current_distance = flow.distance; /* both are in m */ + d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + d.device_id = device_id.devid; + d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + d.variance = 0.0; + + _flow_distance_sensor_pub.publish(d); +} + +void +MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) +{ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; + + vehicle_command_s vcmd{}; + + vcmd.timestamp = hrt_absolute_time(); + + /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = (float)new_mode.base_mode; + vcmd.param2 = (float)custom_mode.main_mode; + vcmd.param3 = (float)custom_mode.sub_mode; + + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = true; + vcmd.from_external = true; + + _cmd_pub.publish(vcmd); +} + +void +MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) +{ + mavlink_distance_sensor_t dist_sensor; + mavlink_msg_distance_sensor_decode(msg, &dist_sensor); + + distance_sensor_s ds{}; + + device::Device::DeviceId device_id; + device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; + device_id.devid_s.address = dist_sensor.id; + + ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */ + ds.min_distance = static_cast(dist_sensor.min_distance) * 1e-2f; /* cm to m */ + ds.max_distance = static_cast(dist_sensor.max_distance) * 1e-2f; /* cm to m */ + ds.current_distance = static_cast(dist_sensor.current_distance) * 1e-2f; /* cm to m */ + ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */ + ds.h_fov = dist_sensor.horizontal_fov; + ds.v_fov = dist_sensor.vertical_fov; + ds.q[0] = dist_sensor.quaternion[0]; + ds.q[1] = dist_sensor.quaternion[1]; + ds.q[2] = dist_sensor.quaternion[2]; + ds.q[3] = dist_sensor.quaternion[3]; + ds.type = dist_sensor.type; + ds.device_id = device_id.devid; + ds.orientation = dist_sensor.orientation; + + // MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown + // quality value. Also it comes normalised between 1 and 100 while the uORB + // signal quality is normalised between 0 and 100. + ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99; + + _distance_sensor_pub.publish(ds); +} + +void +MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) +{ + mavlink_att_pos_mocap_t mocap; + mavlink_msg_att_pos_mocap_decode(msg, &mocap); + + vehicle_odometry_s mocap_odom{}; + + mocap_odom.timestamp = hrt_absolute_time(); + mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec); + + mocap_odom.x = mocap.x; + mocap_odom.y = mocap.y; + mocap_odom.z = mocap.z; + mocap_odom.q[0] = mocap.q[0]; + mocap_odom.q[1] = mocap.q[1]; + mocap_odom.q[2] = mocap.q[2]; + mocap_odom.q[3] = mocap.q[3]; + + const size_t URT_SIZE = sizeof(mocap_odom.pose_covariance) / sizeof(mocap_odom.pose_covariance[0]); + static_assert(URT_SIZE == (sizeof(mocap.covariance) / sizeof(mocap.covariance[0])), + "Odometry Pose Covariance matrix URT array size mismatch"); + + for (size_t i = 0; i < URT_SIZE; i++) { + mocap_odom.pose_covariance[i] = mocap.covariance[i]; + } + + mocap_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; + mocap_odom.vx = NAN; + mocap_odom.vy = NAN; + mocap_odom.vz = NAN; + mocap_odom.rollspeed = NAN; + mocap_odom.pitchspeed = NAN; + mocap_odom.yawspeed = NAN; + mocap_odom.velocity_covariance[0] = NAN; + + _mocap_odometry_pub.publish(mocap_odom); +} + +void +MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg) +{ + mavlink_set_position_target_local_ned_t target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(msg, &target_local_ned); + + /* Only accept messages which are intended for this system */ + if (_mavlink->get_forward_externalsp() && + (mavlink_system.sysid == target_local_ned.target_system || target_local_ned.target_system == 0) && + (mavlink_system.compid == target_local_ned.target_component || target_local_ned.target_component == 0)) { + + vehicle_local_position_setpoint_s setpoint{}; + + const uint16_t type_mask = target_local_ned.type_mask; + + if (target_local_ned.coordinate_frame == MAV_FRAME_LOCAL_NED) { + setpoint.x = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? (float)NAN : target_local_ned.x; + setpoint.y = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? (float)NAN : target_local_ned.y; + setpoint.z = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? (float)NAN : target_local_ned.z; + + setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_local_ned.vx; + setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_local_ned.vy; + setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_local_ned.vz; + + setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_local_ned.afx; + setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_local_ned.afy; + setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? (float)NAN : target_local_ned.afz; + + } else if (target_local_ned.coordinate_frame == MAV_FRAME_BODY_NED) { + + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + const matrix::Dcmf R{matrix::Quatf{vehicle_attitude.q}}; + + const bool ignore_velocity = type_mask & (POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE); + + if (!ignore_velocity) { + const matrix::Vector3f velocity_body_sp{ + (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? 0.f : target_local_ned.vx, + (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? 0.f : target_local_ned.vy, + (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? 0.f : target_local_ned.vz + }; + + + const float yaw = matrix::Eulerf{R}(2); + + setpoint.vx = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1); + setpoint.vy = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1); + setpoint.vz = velocity_body_sp(2); + + } else { + setpoint.vx = NAN; + setpoint.vy = NAN; + setpoint.vz = NAN; + } + + const bool ignore_acceleration = type_mask & (POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE); + + if (!ignore_acceleration) { + const matrix::Vector3f acceleration_body_sp{ + (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? 0.f : target_local_ned.afx, + (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? 0.f : target_local_ned.afy, + (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? 0.f : target_local_ned.afz + }; + + const matrix::Vector3f acceleration_setpoint{R * acceleration_body_sp}; + acceleration_setpoint.copyTo(setpoint.acceleration); + + } else { + setpoint.acceleration[0] = NAN; + setpoint.acceleration[1] = NAN; + setpoint.acceleration[2] = NAN; + } + + setpoint.x = NAN; + setpoint.y = NAN; + setpoint.z = NAN; + + } else { + // mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED coordinate frame %" PRIu8 " unsupported", + // target_local_ned.coordinate_frame); + return; + } + + setpoint.thrust[0] = NAN; + setpoint.thrust[1] = NAN; + setpoint.thrust[2] = NAN; + + setpoint.yaw = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? (float)NAN : target_local_ned.yaw; + setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_local_ned.yaw_rate; + + + offboard_control_mode_s ocm{}; + ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z); + ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz); + ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1]) + || PX4_ISFINITE(setpoint.acceleration[2]); + + if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported"); + return; + } + + if (ocm.position || ocm.velocity || ocm.acceleration) { + // publish offboard_control_mode + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + // only publish setpoint once in OFFBOARD + setpoint.timestamp = hrt_absolute_time(); + _trajectory_setpoint_pub.publish(setpoint); + } + + } else { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED invalid"); + } + } +} + +void +MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t *msg) +{ + mavlink_set_position_target_global_int_t target_global_int; + mavlink_msg_set_position_target_global_int_decode(msg, &target_global_int); + + /* Only accept messages which are intended for this system */ + if (_mavlink->get_forward_externalsp() && + (mavlink_system.sysid == target_global_int.target_system || target_global_int.target_system == 0) && + (mavlink_system.compid == target_global_int.target_component || target_global_int.target_component == 0)) { + + vehicle_local_position_setpoint_s setpoint{}; + + const uint16_t type_mask = target_global_int.type_mask; + + // position + if (!(type_mask & (POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_Z_IGNORE))) { + + vehicle_local_position_s local_pos{}; + _vehicle_local_position_sub.copy(&local_pos); + + if (!local_pos.xy_global || !local_pos.z_global) { + return; + } + + map_projection_reference_s global_local_proj_ref{}; + map_projection_init_timestamped(&global_local_proj_ref, local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp); + + // global -> local + const double lat = target_global_int.lat_int / 1e7; + const double lon = target_global_int.lon_int / 1e7; + map_projection_project(&global_local_proj_ref, lat, lon, &setpoint.x, &setpoint.y); + + if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) { + setpoint.z = local_pos.ref_alt - target_global_int.alt; + + } else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + + if (home_position.valid_alt) { + const float alt = home_position.alt - target_global_int.alt; + setpoint.z = alt - local_pos.ref_alt; + + } else { + // home altitude required + return; + } + + } else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { + vehicle_global_position_s vehicle_global_position{}; + _vehicle_global_position_sub.copy(&vehicle_global_position); + + if (vehicle_global_position.terrain_alt_valid) { + const float alt = target_global_int.alt + vehicle_global_position.terrain_alt; + setpoint.z = local_pos.ref_alt - alt; + + } else { + // valid terrain alt required + return; + } + + } else { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT invalid coordinate frame %" PRIu8, + target_global_int.coordinate_frame); + return; + } + + } else { + setpoint.x = NAN; + setpoint.y = NAN; + setpoint.z = NAN; + } + + // velocity + setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_global_int.vx; + setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_global_int.vy; + setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_global_int.vz; + + // acceleration + setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_global_int.afx; + setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_global_int.afy; + setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? (float)NAN : target_global_int.afz; + + setpoint.thrust[0] = NAN; + setpoint.thrust[1] = NAN; + setpoint.thrust[2] = NAN; + + setpoint.yaw = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? (float)NAN : target_global_int.yaw; + setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)NAN : target_global_int.yaw_rate; + + + offboard_control_mode_s ocm{}; + ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z); + ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz); + ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1]) + || PX4_ISFINITE(setpoint.acceleration[2]); + + if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported"); + return; + } + + if (ocm.position || ocm.velocity || ocm.acceleration) { + // publish offboard_control_mode + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + // only publish setpoint once in OFFBOARD + setpoint.timestamp = hrt_absolute_time(); + _trajectory_setpoint_pub.publish(setpoint); + } + } + } +} + +void +MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg) +{ + // TODO +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + PX4_ERR("SET_ACTUATOR_CONTROL_TARGET not supported with lockstep enabled"); + PX4_ERR("Please disable lockstep for actuator offboard control:"); + PX4_ERR("https://dev.px4.io/master/en/simulation/#disable-lockstep-simulation"); + return; +#endif + + mavlink_set_actuator_control_target_t actuator_target; + mavlink_msg_set_actuator_control_target_decode(msg, &actuator_target); + + if (_mavlink->get_forward_externalsp() && + (mavlink_system.sysid == actuator_target.target_system || actuator_target.target_system == 0) && + (mavlink_system.compid == actuator_target.target_component || actuator_target.target_component == 0) + ) { + /* Ignore all setpoints except when controlling the gimbal(group_mlx==2) as we are setting raw actuators here */ + //bool ignore_setpoints = bool(actuator_target.group_mlx != 2); + + offboard_control_mode_s offboard_control_mode{}; + offboard_control_mode.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(offboard_control_mode); + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + // Publish actuator controls only once in OFFBOARD + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + + actuator_controls_s actuator_controls{}; + actuator_controls.timestamp = hrt_absolute_time(); + + /* Set duty cycles for the servos in the actuator_controls message */ + for (size_t i = 0; i < 8; i++) { + actuator_controls.control[i] = actuator_target.controls[i]; + } + + switch (actuator_target.group_mlx) { + case 0: + _actuator_controls_pubs[0].publish(actuator_controls); + break; + + case 1: + _actuator_controls_pubs[1].publish(actuator_controls); + break; + + case 2: + _actuator_controls_pubs[2].publish(actuator_controls); + break; + + case 3: + _actuator_controls_pubs[3].publish(actuator_controls); + break; + + default: + break; + } + } + } +} + +void +MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg) +{ + mavlink_set_gps_global_origin_t gps_global_origin; + mavlink_msg_set_gps_global_origin_decode(msg, &gps_global_origin); + + if (gps_global_origin.target_system == _mavlink->get_system_id()) { + vehicle_command_s vcmd{}; + vcmd.param5 = (double)gps_global_origin.latitude * 1.e-7; + vcmd.param6 = (double)gps_global_origin.longitude * 1.e-7; + vcmd.param7 = (float)gps_global_origin.altitude * 1.e-3f; + vcmd.command = vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN; + vcmd.target_system = _mavlink->get_system_id(); + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = false; + vcmd.from_external = true; + vcmd.timestamp = hrt_absolute_time(); + _cmd_pub.publish(vcmd); + } + + handle_request_message_command(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN); +} + +void +MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) +{ + mavlink_vision_position_estimate_t ev; + mavlink_msg_vision_position_estimate_decode(msg, &ev); + + vehicle_odometry_s visual_odom{}; + + visual_odom.timestamp = hrt_absolute_time(); + visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec); + + visual_odom.x = ev.x; + visual_odom.y = ev.y; + visual_odom.z = ev.z; + matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw)); + q.copyTo(visual_odom.q); + + visual_odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + const size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_covariance[0]); + static_assert(URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])), + "Odometry Pose Covariance matrix URT array size mismatch"); + + for (size_t i = 0; i < URT_SIZE; i++) { + visual_odom.pose_covariance[i] = ev.covariance[i]; + } + + visual_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; + visual_odom.vx = NAN; + visual_odom.vy = NAN; + visual_odom.vz = NAN; + visual_odom.rollspeed = NAN; + visual_odom.pitchspeed = NAN; + visual_odom.yawspeed = NAN; + visual_odom.velocity_covariance[0] = NAN; + + _visual_odometry_pub.publish(visual_odom); +} + +void +MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) +{ + mavlink_odometry_t odom; + mavlink_msg_odometry_decode(msg, &odom); + + vehicle_odometry_s odometry{}; + + odometry.timestamp = hrt_absolute_time(); + odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec); + + /* The position is in a local FRD frame */ + odometry.x = odom.x; + odometry.y = odom.y; + odometry.z = odom.z; + + /** + * The quaternion of the ODOMETRY msg represents a rotation from body frame + * to a local frame + */ + matrix::Quatf q_body_to_local(odom.q); + q_body_to_local.normalize(); + q_body_to_local.copyTo(odometry.q); + + // pose_covariance + static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]); + static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])), + "Odometry Pose Covariance matrix URT array size mismatch"); + + // velocity_covariance + static constexpr size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]); + static_assert(VEL_URT_SIZE == (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])), + "Odometry Velocity Covariance matrix URT array size mismatch"); + + // TODO: create a method to simplify covariance copy + for (size_t i = 0; i < POS_URT_SIZE; i++) { + odometry.pose_covariance[i] = odom.pose_covariance[i]; + } + + /** + * PX4 expects the body's linear velocity in the local frame, + * the linear velocity is rotated from the odom child_frame to the + * local NED frame. The angular velocity needs to be expressed in the + * body (fcu_frd) frame. + */ + if (odom.child_frame_id == MAV_FRAME_BODY_FRD) { + + odometry.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD; + odometry.vx = odom.vx; + odometry.vy = odom.vy; + odometry.vz = odom.vz; + + odometry.rollspeed = odom.rollspeed; + odometry.pitchspeed = odom.pitchspeed; + odometry.yawspeed = odom.yawspeed; + + for (size_t i = 0; i < VEL_URT_SIZE; i++) { + odometry.velocity_covariance[i] = odom.velocity_covariance[i]; + } + + } else { + PX4_ERR("Body frame %" PRIu8 " not supported. Unable to publish velocity", odom.child_frame_id); + } + + /** + * Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD + * The supported sources of the data/tesimator type are MAV_ESTIMATOR_TYPE_VISION, + * MAV_ESTIMATOR_TYPE_VIO and MAV_ESTIMATOR_TYPE_MOCAP + * + * @note Regarding the local frames of reference, the appropriate EKF_AID_MASK + * should be set in order to match a frame aligned (NED) or not aligned (FRD) + * with true North + */ + if (odom.frame_id == MAV_FRAME_LOCAL_NED || odom.frame_id == MAV_FRAME_LOCAL_FRD) { + + if (odom.frame_id == MAV_FRAME_LOCAL_NED) { + odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + } else { + odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; + } + + if ((odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION) + || (odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO) + || (odom.estimator_type == MAV_ESTIMATOR_TYPE_UNKNOWN)) { + // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support + _visual_odometry_pub.publish(odometry); + + } else if (odom.estimator_type == MAV_ESTIMATOR_TYPE_MOCAP) { + _mocap_odometry_pub.publish(odometry); + + } else { + PX4_ERR("Estimator source %" PRIu8 " not supported. Unable to publish pose and velocity", odom.estimator_type); + } + + } else { + PX4_ERR("Local frame %" PRIu8 " not supported. Unable to publish pose and velocity", odom.frame_id); + } +} + +void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust) +{ + // Fill correct field by checking frametype + // TODO: add as needed + switch (_mavlink->get_system_type()) { + case MAV_TYPE_GENERIC: + break; + + case MAV_TYPE_FIXED_WING: + case MAV_TYPE_GROUND_ROVER: + thrust_body_array[0] = thrust; + break; + + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: + case MAV_TYPE_TRICOPTER: + case MAV_TYPE_HELICOPTER: + case MAV_TYPE_COAXIAL: + thrust_body_array[2] = -thrust; + break; + + case MAV_TYPE_SUBMARINE: + thrust_body_array[0] = thrust; + break; + + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + case MAV_TYPE_VTOL_RESERVED2: + case MAV_TYPE_VTOL_RESERVED3: + case MAV_TYPE_VTOL_RESERVED4: + case MAV_TYPE_VTOL_RESERVED5: + switch (vehicle_type) { + case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: + thrust_body_array[0] = thrust; + + break; + + case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: + thrust_body_array[2] = -thrust; + + break; + + default: + // This should never happen + break; + } + + break; + } +} + +void +MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) +{ + mavlink_set_attitude_target_t attitude_target; + mavlink_msg_set_attitude_target_decode(msg, &attitude_target); + + /* Only accept messages which are intended for this system */ + if (_mavlink->get_forward_externalsp() && + (mavlink_system.sysid == attitude_target.target_system || attitude_target.target_system == 0) && + (mavlink_system.compid == attitude_target.target_component || attitude_target.target_component == 0)) { + + const uint8_t type_mask = attitude_target.type_mask; + + const bool attitude = !(type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE); + const bool body_rates = !(type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE) + && !(type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE); + const bool thrust_body = (type_mask & ATTITUDE_TARGET_TYPEMASK_THRUST_BODY_SET); + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + if (attitude) { + vehicle_attitude_setpoint_s attitude_setpoint{}; + + const matrix::Quatf q{attitude_target.q}; + q.copyTo(attitude_setpoint.q_d); + + matrix::Eulerf euler{q}; + attitude_setpoint.roll_body = euler.phi(); + attitude_setpoint.pitch_body = euler.theta(); + attitude_setpoint.yaw_body = euler.psi(); + + // TODO: review use case + attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? + (float)NAN : attitude_target.body_yaw_rate; + + if (!thrust_body && !(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) { + fill_thrust(attitude_setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust); + + } else if (thrust_body) { + attitude_setpoint.thrust_body[0] = attitude_target.thrust_body[0]; + attitude_setpoint.thrust_body[1] = attitude_target.thrust_body[1]; + attitude_setpoint.thrust_body[2] = attitude_target.thrust_body[2]; + } + + // publish offboard_control_mode + offboard_control_mode_s ocm{}; + ocm.attitude = true; + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + // Publish attitude setpoint only once in OFFBOARD + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + attitude_setpoint.timestamp = hrt_absolute_time(); + + if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { + _mc_virtual_att_sp_pub.publish(attitude_setpoint); + + } else if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + _fw_virtual_att_sp_pub.publish(attitude_setpoint); + + } else { + _att_sp_pub.publish(attitude_setpoint); + } + } + + } else if (body_rates) { + vehicle_rates_setpoint_s setpoint{}; + setpoint.roll = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE) ? (float)NAN : + attitude_target.body_roll_rate; + setpoint.pitch = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE) ? (float)NAN : + attitude_target.body_pitch_rate; + setpoint.yaw = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? (float)NAN : + attitude_target.body_yaw_rate; + + if (!(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) { + fill_thrust(setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust); + } + + // publish offboard_control_mode + offboard_control_mode_s ocm{}; + ocm.body_rate = true; + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + // Publish rate setpoint only once in OFFBOARD + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + setpoint.timestamp = hrt_absolute_time(); + _rates_sp_pub.publish(setpoint); + } + } + } +} + +void +MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) +{ + /* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ + if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + radio_status_s status{}; + + status.timestamp = hrt_absolute_time(); + status.rssi = rstatus.rssi; + status.remote_rssi = rstatus.remrssi; + status.txbuf = rstatus.txbuf; + status.noise = rstatus.noise; + status.remote_noise = rstatus.remnoise; + status.rxerrors = rstatus.rxerrors; + status.fix = rstatus.fixed; + + _mavlink->update_radio_status(status); + + _radio_status_pub.publish(status); + } +} + +void +MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) +{ + mavlink_ping_t ping; + mavlink_msg_ping_decode(msg, &ping); + + if ((ping.target_system == 0) && + (ping.target_component == 0)) { // This is a ping request. Return it to the system which requested the ping. + + ping.target_system = msg->sysid; + ping.target_component = msg->compid; + mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping); + + } else if ((ping.target_system == mavlink_system.sysid) && + (ping.target_component == + mavlink_system.compid)) { // This is a returned ping message from this system. Calculate latency from it. + + const hrt_abstime now = hrt_absolute_time(); + + // Calculate round trip time + float rtt_ms = (now - ping.time_usec) / 1000.0f; + + // Update ping statistics + struct Mavlink::ping_statistics_s &pstats = _mavlink->get_ping_statistics(); + + pstats.last_ping_time = now; + + if (pstats.last_ping_seq == 0 && ping.seq > 0) { + // This is the first reply we are receiving from an offboard system. + // We may have been broadcasting pings for some time before it came online, + // and these do not count as dropped packets. + + // Reset last_ping_seq counter for correct packet drop detection + pstats.last_ping_seq = ping.seq - 1; + } + + // We can only count dropped packets after the first message + if (ping.seq > pstats.last_ping_seq) { + pstats.dropped_packets += ping.seq - pstats.last_ping_seq - 1; + } + + pstats.last_ping_seq = ping.seq; + pstats.last_rtt = rtt_ms; + pstats.mean_rtt = (rtt_ms + pstats.mean_rtt) / 2.0f; + pstats.max_rtt = fmaxf(rtt_ms, pstats.max_rtt); + pstats.min_rtt = pstats.min_rtt > 0.0f ? fminf(rtt_ms, pstats.min_rtt) : rtt_ms; + + /* Ping status is supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ + if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { + + ping_s uorb_ping_msg{}; + + uorb_ping_msg.timestamp = now; + uorb_ping_msg.ping_time = ping.time_usec; + uorb_ping_msg.ping_sequence = ping.seq; + uorb_ping_msg.dropped_packets = pstats.dropped_packets; + uorb_ping_msg.rtt_ms = rtt_ms; + uorb_ping_msg.system_id = msg->sysid; + uorb_ping_msg.component_id = msg->compid; + + _ping_pub.publish(uorb_ping_msg); + } + } +} + +void +MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) +{ + if ((msg->sysid != mavlink_system.sysid) || (msg->compid == mavlink_system.compid)) { + // ignore battery status coming from other systems or from the autopilot itself + return; + } + + // external battery measurements + mavlink_battery_status_t battery_mavlink; + mavlink_msg_battery_status_decode(msg, &battery_mavlink); + + battery_status_s battery_status{}; + battery_status.timestamp = hrt_absolute_time(); + + float voltage_sum = 0.0f; + uint8_t cell_count = 0; + + while (battery_mavlink.voltages[cell_count] < UINT16_MAX && cell_count < 10) { + battery_status.voltage_cell_v[cell_count] = (float)(battery_mavlink.voltages[cell_count]) / 1000.0f; + voltage_sum += battery_status.voltage_cell_v[cell_count]; + cell_count++; + } + + battery_status.voltage_v = voltage_sum; + battery_status.voltage_filtered_v = voltage_sum; + battery_status.current_a = battery_status.current_filtered_a = (float)(battery_mavlink.current_battery) / 100.0f; + battery_status.current_filtered_a = battery_status.current_a; + battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f; + battery_status.discharged_mah = (float)battery_mavlink.current_consumed; + battery_status.cell_count = cell_count; + battery_status.temperature = (float)battery_mavlink.temperature; + battery_status.connected = true; + + // Set the battery warning based on remaining charge. + // Note: Smallest values must come first in evaluation. + if (battery_status.remaining < _param_bat_emergen_thr.get()) { + battery_status.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + + } else if (battery_status.remaining < _param_bat_crit_thr.get()) { + battery_status.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else if (battery_status.remaining < _param_bat_low_thr.get()) { + battery_status.warning = battery_status_s::BATTERY_WARNING_LOW; + } + + _battery_pub.publish(battery_status); +} + +void +MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg) +{ + mavlink_serial_control_t serial_control_mavlink; + mavlink_msg_serial_control_decode(msg, &serial_control_mavlink); + + // we only support shell commands + if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL + || (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) { + return; + } + + MavlinkShell *shell = _mavlink->get_shell(); + + if (shell) { + // we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message + if (serial_control_mavlink.count > 0) { + shell->write(serial_control_mavlink.data, serial_control_mavlink.count); + } + + // if no response requested, assume the shell is no longer used + if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) { + _mavlink->close_shell(); + } + } +} + +void +MavlinkReceiver::handle_message_logging_ack(mavlink_message_t *msg) +{ + mavlink_logging_ack_t logging_ack; + mavlink_msg_logging_ack_decode(msg, &logging_ack); + + MavlinkULog *ulog_streaming = _mavlink->get_ulog_streaming(); + + if (ulog_streaming) { + ulog_streaming->handle_ack(logging_ack); + } +} + +void +MavlinkReceiver::handle_message_play_tune(mavlink_message_t *msg) +{ + mavlink_play_tune_t play_tune; + mavlink_msg_play_tune_decode(msg, &play_tune); + + if ((mavlink_system.sysid == play_tune.target_system || play_tune.target_system == 0) && + (mavlink_system.compid == play_tune.target_component || play_tune.target_component == 0)) { + + // Let's make sure the input is 0 terminated, so we don't ever overrun it. + play_tune.tune2[sizeof(play_tune.tune2) - 1] = '\0'; + + schedule_tune(play_tune.tune); + } +} + +void +MavlinkReceiver::handle_message_play_tune_v2(mavlink_message_t *msg) +{ + mavlink_play_tune_v2_t play_tune_v2; + mavlink_msg_play_tune_v2_decode(msg, &play_tune_v2); + + if ((mavlink_system.sysid == play_tune_v2.target_system || play_tune_v2.target_system == 0) && + (mavlink_system.compid == play_tune_v2.target_component || play_tune_v2.target_component == 0)) { + + if (play_tune_v2.format != TUNE_FORMAT_QBASIC1_1) { + PX4_ERR("Tune format %" PRIu32 " not supported", play_tune_v2.format); + return; + } + + // Let's make sure the input is 0 terminated, so we don't ever overrun it. + play_tune_v2.tune[sizeof(play_tune_v2.tune) - 1] = '\0'; + + schedule_tune(play_tune_v2.tune); + } +} + +void MavlinkReceiver::schedule_tune(const char *tune) +{ + // We only allocate the TunePublisher object if we ever use it but we + // don't remove it to avoid fragmentation over time. + if (_tune_publisher == nullptr) { + _tune_publisher = new TunePublisher(); + + if (_tune_publisher == nullptr) { + PX4_ERR("Could not allocate tune publisher"); + return; + } + } + + const hrt_abstime now = hrt_absolute_time(); + + _tune_publisher->set_tune_string(tune, now); + // Send first one straightaway. + _tune_publisher->publish_next_tune(now); +} + + +void +MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg) +{ + mavlink_obstacle_distance_t mavlink_obstacle_distance; + mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance); + + obstacle_distance_s obstacle_distance{}; + + obstacle_distance.timestamp = hrt_absolute_time(); + obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type; + memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances)); + + if (mavlink_obstacle_distance.increment_f > 0.f) { + obstacle_distance.increment = mavlink_obstacle_distance.increment_f; + + } else { + obstacle_distance.increment = (float)mavlink_obstacle_distance.increment; + } + + obstacle_distance.min_distance = mavlink_obstacle_distance.min_distance; + obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance; + obstacle_distance.angle_offset = mavlink_obstacle_distance.angle_offset; + obstacle_distance.frame = mavlink_obstacle_distance.frame; + + _obstacle_distance_pub.publish(obstacle_distance); +} + +void +MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message_t *msg) +{ + mavlink_trajectory_representation_bezier_t trajectory; + mavlink_msg_trajectory_representation_bezier_decode(msg, &trajectory); + + vehicle_trajectory_bezier_s trajectory_bezier{}; + + trajectory_bezier.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec); + + for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) { + trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i]; + trajectory_bezier.control_points[i].position[1] = trajectory.pos_y[i]; + trajectory_bezier.control_points[i].position[2] = trajectory.pos_z[i]; + + trajectory_bezier.control_points[i].delta = trajectory.delta[i]; + trajectory_bezier.control_points[i].yaw = trajectory.pos_yaw[i]; + } + + trajectory_bezier.bezier_order = math::min(trajectory.valid_points, vehicle_trajectory_bezier_s::NUMBER_POINTS); + _trajectory_bezier_pub.publish(trajectory_bezier); +} + +void +MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_message_t *msg) +{ + mavlink_trajectory_representation_waypoints_t trajectory; + mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory); + + vehicle_trajectory_waypoint_s trajectory_waypoint{}; + + trajectory_waypoint.timestamp = hrt_absolute_time(); + const int number_valid_points = trajectory.valid_points; + + for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { + trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i]; + trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i]; + trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i]; + + trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i]; + trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i]; + trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i]; + + trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i]; + trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i]; + trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i]; + + trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i]; + trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; + + trajectory_waypoint.waypoints[i].type = UINT8_MAX; + } + + for (int i = 0; i < number_valid_points; ++i) { + trajectory_waypoint.waypoints[i].point_valid = true; + } + + for (int i = number_valid_points; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { + trajectory_waypoint.waypoints[i].point_valid = false; + } + + _trajectory_waypoint_pub.publish(trajectory_waypoint); +} + +int +MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw) +{ + bool on = (buttons & (1 << sw)); + + if (sw < MOM_SWITCH_COUNT) { + + bool last_on = (_mom_switch_state & (1 << sw)); + + /* first switch is 2-pos, rest is 2 pos */ + unsigned state_count = (sw == 0) ? 3 : 2; + + /* only transition on low state */ + if (!on && (on != last_on)) { + + _mom_switch_pos[sw]++; + + if (_mom_switch_pos[sw] == state_count) { + _mom_switch_pos[sw] = 0; + } + } + + /* state_count - 1 is the number of intervals and 1000 is the range, + * with 2 states 0 becomes 0, 1 becomes 1000. With + * 3 states 0 becomes 0, 1 becomes 500, 2 becomes 1000, + * and so on for more states. + */ + return (_mom_switch_pos[sw] * 1000) / (state_count - 1) + 1000; + + } else { + /* return the current state */ + return on * 1000 + 1000; + } +} + +void +MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) +{ + mavlink_rc_channels_override_t man; + mavlink_msg_rc_channels_override_decode(msg, &man); + + // Check target + if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) { + return; + } + + // fill uORB message + input_rc_s rc{}; + + // metadata + rc.timestamp = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp; + rc.rssi = RC_INPUT_RSSI_MAX; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + + // channels + rc.values[0] = man.chan1_raw; + rc.values[1] = man.chan2_raw; + rc.values[2] = man.chan3_raw; + rc.values[3] = man.chan4_raw; + rc.values[4] = man.chan5_raw; + rc.values[5] = man.chan6_raw; + rc.values[6] = man.chan7_raw; + rc.values[7] = man.chan8_raw; + rc.values[8] = man.chan9_raw; + rc.values[9] = man.chan10_raw; + rc.values[10] = man.chan11_raw; + rc.values[11] = man.chan12_raw; + rc.values[12] = man.chan13_raw; + rc.values[13] = man.chan14_raw; + rc.values[14] = man.chan15_raw; + rc.values[15] = man.chan16_raw; + rc.values[16] = man.chan17_raw; + rc.values[17] = man.chan18_raw; + + // check how many channels are valid + for (int i = 17; i >= 0; i--) { + const bool ignore_max = rc.values[i] == UINT16_MAX; // ignore any channel with value UINT16_MAX + const bool ignore_zero = (i > 7) && (rc.values[i] == 0); // ignore channel 8-18 if value is 0 + + if (ignore_max || ignore_zero) { + // set all ignored values to zero + rc.values[i] = 0; + + } else { + // first channel to not ignore -> set count considering zero-based index + rc.channel_count = i + 1; + break; + } + } + + // publish uORB message + _rc_pub.publish(rc); +} + +void +MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) +{ + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + // Check target + if (man.target != 0 && man.target != _mavlink->get_system_id()) { + return; + } + + if (_mavlink->should_generate_virtual_rc_input()) { + + input_rc_s rc{}; + rc.timestamp = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp; + + rc.channel_count = 8; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + rc.rssi = RC_INPUT_RSSI_MAX; + + rc.values[0] = man.x / 2 + 1500; // roll + rc.values[1] = man.y / 2 + 1500; // pitch + rc.values[2] = man.r / 2 + 1500; // yaw + rc.values[3] = math::constrain(man.z / 0.9f + 800.0f, 1000.0f, 2000.0f); // throttle + + /* decode all switches which fit into the channel mask */ + unsigned max_switch = (sizeof(man.buttons) * 8); + unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0])); + + if (max_switch > (max_channels - 4)) { + max_switch = (max_channels - 4); + } + + /* fill all channels */ + for (unsigned i = 0; i < max_switch; i++) { + rc.values[i + 4] = decode_switch_pos_n(man.buttons, i); + } + + _mom_switch_state = man.buttons; + + _rc_pub.publish(rc); + + } else { + manual_control_setpoint_s manual{}; + + manual.timestamp = hrt_absolute_time(); + manual.x = man.x / 1000.0f; + manual.y = man.y / 1000.0f; + manual.r = man.r / 1000.0f; + manual.z = man.z / 1000.0f; + manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); + + _manual_control_setpoint_pub.publish(manual); + } +} + +void +MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) +{ + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { + + const hrt_abstime now = hrt_absolute_time(); + + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); + + const bool same_system = (msg->sysid == mavlink_system.sysid); + + if (same_system || hb.type == MAV_TYPE_GCS) { + + switch (hb.type) { + case MAV_TYPE_ANTENNA_TRACKER: + _heartbeat_type_antenna_tracker = now; + break; + + case MAV_TYPE_GCS: + _heartbeat_type_gcs = now; + break; + + case MAV_TYPE_ONBOARD_CONTROLLER: + _heartbeat_type_onboard_controller = now; + break; + + case MAV_TYPE_GIMBAL: + _heartbeat_type_gimbal = now; + break; + + case MAV_TYPE_ADSB: + _heartbeat_type_adsb = now; + break; + + case MAV_TYPE_CAMERA: + _heartbeat_type_camera = now; + break; + + default: + PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %" PRIu8 " from SYSID: %" PRIu8 ", COMPID: %" PRIu8, hb.type, msg->sysid, + msg->compid); + } + + + switch (msg->compid) { + case MAV_COMP_ID_TELEMETRY_RADIO: + _heartbeat_component_telemetry_radio = now; + break; + + case MAV_COMP_ID_LOG: + _heartbeat_component_log = now; + break; + + case MAV_COMP_ID_OSD: + _heartbeat_component_osd = now; + break; + + case MAV_COMP_ID_OBSTACLE_AVOIDANCE: + _heartbeat_component_obstacle_avoidance = now; + _mavlink->telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE); + break; + + case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY: + _heartbeat_component_visual_inertial_odometry = now; + break; + + case MAV_COMP_ID_PAIRING_MANAGER: + _heartbeat_component_pairing_manager = now; + break; + + case MAV_COMP_ID_UDP_BRIDGE: + _heartbeat_component_udp_bridge = now; + break; + + case MAV_COMP_ID_UART_BRIDGE: + _heartbeat_component_uart_bridge = now; + break; + + default: + PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %" PRIu8 " from SYSID: %" PRIu8 ", COMPID: %" PRIu8, hb.type, msg->sysid, + msg->compid); + } + + CheckHeartbeats(now, true); + } + } +} + +int +MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate) +{ + if (msgId == MAVLINK_MSG_ID_HEARTBEAT) { + return PX4_ERROR; + } + + if (data_rate > 0) { + _mavlink->set_data_rate(data_rate); + } + + // configure_stream wants a rate (msgs/second), so convert here. + float rate = 0.f; + + if (interval < -0.00001f) { + rate = 0.f; // stop the stream + + } else if (interval > 0.00001f) { + rate = 1000000.0f / interval; + + } else { + rate = -2.f; // set default rate + } + + bool found_id = false; + + if (msgId != 0) { + const char *stream_name = get_stream_name(msgId); + + if (stream_name != nullptr) { + _mavlink->configure_stream_threadsafe(stream_name, rate); + found_id = true; + } + } + + return (found_id ? PX4_OK : PX4_ERROR); +} + +void +MavlinkReceiver::get_message_interval(int msgId) +{ + unsigned interval = 0; + + for (const auto &stream : _mavlink->get_streams()) { + if (stream->get_id() == msgId) { + interval = stream->get_interval(); + break; + } + } + + // send back this value... + mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval); +} + +void +MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) +{ + mavlink_hil_sensor_t hil_sensor; + mavlink_msg_hil_sensor_decode(msg, &hil_sensor); + + const uint64_t timestamp = hrt_absolute_time(); + + // temperature only updated with baro + float temperature = NAN; + + if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) { + temperature = hil_sensor.temperature; + } + + // gyro + if ((hil_sensor.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) { + if (_px4_gyro == nullptr) { + // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_gyro = new PX4Gyroscope(1310988); + } + + if (_px4_gyro != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_gyro->set_temperature(temperature); + } + + _px4_gyro->update(timestamp, hil_sensor.xgyro, hil_sensor.ygyro, hil_sensor.zgyro); + } + } + + // accelerometer + if ((hil_sensor.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) { + if (_px4_accel == nullptr) { + // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_accel = new PX4Accelerometer(1310988); + } + + if (_px4_accel != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_accel->set_temperature(temperature); + } + + _px4_accel->update(timestamp, hil_sensor.xacc, hil_sensor.yacc, hil_sensor.zacc); + } + } + + // magnetometer + if ((hil_sensor.fields_updated & SensorSource::MAG) == SensorSource::MAG) { + if (_px4_mag == nullptr) { + // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + _px4_mag = new PX4Magnetometer(197388); + } + + if (_px4_mag != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_mag->set_temperature(temperature); + } + + _px4_mag->update(timestamp, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag); + } + } + + // baro + if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) { + if (_px4_baro == nullptr) { + // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + _px4_baro = new PX4Barometer(6620172); + } + + if (_px4_baro != nullptr) { + _px4_baro->set_temperature(hil_sensor.temperature); + _px4_baro->update(timestamp, hil_sensor.abs_pressure); + } + } + + // differential pressure + if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) { + differential_pressure_s report{}; + report.timestamp = timestamp; + report.temperature = hil_sensor.temperature; + report.differential_pressure_filtered_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar; + report.differential_pressure_raw_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar; + + _differential_pressure_pub.publish(report); + } + + // battery status + { + battery_status_s hil_battery_status{}; + + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 16.0f; + hil_battery_status.voltage_filtered_v = 16.0f; + hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; + hil_battery_status.connected = true; + hil_battery_status.remaining = 0.70; + + _battery_pub.publish(hil_battery_status); + } +} + +void +MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) +{ + mavlink_hil_gps_t hil_gps; + mavlink_msg_hil_gps_decode(msg, &hil_gps); + + sensor_gps_s gps{}; + + device::Device::DeviceId device_id{}; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + gps.device_id = device_id.devid; + + gps.lat = hil_gps.lat; + gps.lon = hil_gps.lon; + gps.alt = hil_gps.alt; + gps.alt_ellipsoid = hil_gps.alt; + + gps.s_variance_m_s = 0.25f; + gps.c_variance_rad = 0.5f; + gps.fix_type = hil_gps.fix_type; + + gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m + gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m + + gps.hdop = 0; // TODO + gps.vdop = 0; // TODO + + gps.noise_per_ms = 0; + gps.automatic_gain_control = 0; + gps.jamming_indicator = 0; + gps.jamming_state = 0; + + gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s + gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s + gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s + gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s + gps.cog_rad = ((hil_gps.cog == 65535) ? (float)NAN : matrix::wrap_2pi(math::radians( + hil_gps.cog * 1e-2f))); // cdeg -> rad + gps.vel_ned_valid = true; + + gps.timestamp_time_relative = 0; + gps.time_utc_usec = hil_gps.time_usec; + + gps.satellites_used = hil_gps.satellites_visible; + + gps.heading = NAN; + gps.heading_offset = NAN; + + gps.timestamp = hrt_absolute_time(); + + _sensor_gps_pub.publish(gps); +} + +void +MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) +{ + mavlink_follow_target_t follow_target_msg; + mavlink_msg_follow_target_decode(msg, &follow_target_msg); + + follow_target_s follow_target_topic{}; + + follow_target_topic.timestamp = hrt_absolute_time(); + follow_target_topic.lat = follow_target_msg.lat * 1e-7; + follow_target_topic.lon = follow_target_msg.lon * 1e-7; + follow_target_topic.alt = follow_target_msg.alt; + follow_target_topic.vx = follow_target_msg.vel[0]; + follow_target_topic.vy = follow_target_msg.vel[1]; + follow_target_topic.vz = follow_target_msg.vel[2]; + + _follow_target_pub.publish(follow_target_topic); +} + +void +MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg) +{ + mavlink_landing_target_t landing_target; + mavlink_msg_landing_target_decode(msg, &landing_target); + + if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) { + landing_target_pose_s landing_target_pose{}; + + landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec); + landing_target_pose.abs_pos_valid = true; + landing_target_pose.x_abs = landing_target.x; + landing_target_pose.y_abs = landing_target.y; + landing_target_pose.z_abs = landing_target.z; + + _landing_target_pose_pub.publish(landing_target_pose); + + } else if (landing_target.position_valid) { + // We only support MAV_FRAME_LOCAL_NED. In this case, the frame was unsupported. + mavlink_log_critical(&_mavlink_log_pub, "landing target: coordinate frame %" PRIu8 " unsupported", + landing_target.frame); + + } else { + irlock_report_s irlock_report{}; + + irlock_report.timestamp = hrt_absolute_time(); + irlock_report.signature = landing_target.target_num; + irlock_report.pos_x = landing_target.angle_x; + irlock_report.pos_y = landing_target.angle_y; + irlock_report.size_x = landing_target.size_x; + irlock_report.size_y = landing_target.size_y; + + _irlock_report_pub.publish(irlock_report); + } +} + +void +MavlinkReceiver::handle_message_cellular_status(mavlink_message_t *msg) +{ + mavlink_cellular_status_t status; + mavlink_msg_cellular_status_decode(msg, &status); + + cellular_status_s cellular_status{}; + + cellular_status.timestamp = hrt_absolute_time(); + cellular_status.status = status.status; + cellular_status.failure_reason = status.failure_reason; + cellular_status.type = status.type; + cellular_status.quality = status.quality; + cellular_status.mcc = status.mcc; + cellular_status.mnc = status.mnc; + cellular_status.lac = status.lac; + + _cellular_status_pub.publish(cellular_status); +} + +void +MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) +{ + mavlink_adsb_vehicle_t adsb; + mavlink_msg_adsb_vehicle_decode(msg, &adsb); + + transponder_report_s t{}; + + t.timestamp = hrt_absolute_time(); + + t.icao_address = adsb.ICAO_address; + t.lat = adsb.lat * 1e-7; + t.lon = adsb.lon * 1e-7; + t.altitude_type = adsb.altitude_type; + t.altitude = adsb.altitude / 1000.0f; + t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F - M_PI_F; + t.hor_velocity = adsb.hor_velocity / 100.0f; + t.ver_velocity = adsb.ver_velocity / 100.0f; + memcpy(&t.callsign[0], &adsb.callsign[0], sizeof(t.callsign)); + t.emitter_type = adsb.emitter_type; + t.tslc = adsb.tslc; + t.squawk = adsb.squawk; + + t.flags = transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE; //Unset in receiver already broadcast its messages + + if (adsb.flags & ADSB_FLAGS_VALID_COORDS) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; } + + if (adsb.flags & ADSB_FLAGS_VALID_ALTITUDE) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; } + + if (adsb.flags & ADSB_FLAGS_VALID_HEADING) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; } + + if (adsb.flags & ADSB_FLAGS_VALID_VELOCITY) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; } + + if (adsb.flags & ADSB_FLAGS_VALID_CALLSIGN) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; } + + if (adsb.flags & ADSB_FLAGS_VALID_SQUAWK) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK; } + + //PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc); + + _transponder_report_pub.publish(t); +} + +void +MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) +{ + mavlink_utm_global_position_t utm_pos; + mavlink_msg_utm_global_position_decode(msg, &utm_pos); + + bool is_self_published = false; + + +#ifndef BOARD_HAS_NO_UUID + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + is_self_published = sizeof(px4_guid) == sizeof(utm_pos.uas_id) + && memcmp(px4_guid, utm_pos.uas_id, sizeof(px4_guid_t)) == 0; +#else + + is_self_published = msg->sysid == _mavlink->get_system_id(); +#endif /* BOARD_HAS_NO_UUID */ + + + //Ignore selfpublished UTM messages + if (is_self_published) { + return; + } + + // Convert cm/s to m/s + float vx = utm_pos.vx / 100.0f; + float vy = utm_pos.vy / 100.0f; + float vz = utm_pos.vz / 100.0f; + + transponder_report_s t{}; + t.timestamp = hrt_absolute_time(); + mav_array_memcpy(t.uas_id, utm_pos.uas_id, PX4_GUID_BYTE_LENGTH); + t.lat = utm_pos.lat * 1e-7; + t.lon = utm_pos.lon * 1e-7; + t.altitude = utm_pos.alt / 1000.0f; + t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; + // UTM_GLOBAL_POSIION uses NED (north, east, down) coordinates for velocity, in cm / s. + t.heading = atan2f(vy, vx); + t.hor_velocity = sqrtf(vy * vy + vx * vx); + t.ver_velocity = -vz; + // TODO: Callsign + // For now, set it to all 0s. This is a null-terminated string, so not explicitly giving it a null + // terminator could cause problems. + memset(&t.callsign[0], 0, sizeof(t.callsign)); + t.emitter_type = ADSB_EMITTER_TYPE_UAV; // TODO: Is this correct?x2? + + // The Mavlink docs do not specify what to do if tslc (time since last communication) is out of range of + // an 8-bit int, or if this is the first communication. + // Here, I assume that if this is the first communication, tslc = 0. + // If tslc > 255, then tslc = 255. + unsigned long time_passed = (t.timestamp - _last_utm_global_pos_com) / 1000000; + + if (_last_utm_global_pos_com == 0) { + time_passed = 0; + + } else if (time_passed > UINT8_MAX) { + time_passed = UINT8_MAX; + } + + t.tslc = (uint8_t) time_passed; + + t.flags = 0; + + if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; + } + + if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; + } + + if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + } + + // Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not + // provide these. + _transponder_report_pub.publish(t); + + _last_utm_global_pos_com = t.timestamp; +} + +void +MavlinkReceiver::handle_message_collision(mavlink_message_t *msg) +{ + mavlink_collision_t collision; + mavlink_msg_collision_decode(msg, &collision); + + collision_report_s collision_report{}; + + collision_report.timestamp = hrt_absolute_time(); + collision_report.src = collision.src; + collision_report.id = collision.id; + collision_report.action = collision.action; + collision_report.threat_level = collision.threat_level; + collision_report.time_to_minimum_delta = collision.time_to_minimum_delta; + collision_report.altitude_minimum_delta = collision.altitude_minimum_delta; + collision_report.horizontal_minimum_delta = collision.horizontal_minimum_delta; + + _collision_report_pub.publish(collision_report); +} + +void +MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg) +{ + mavlink_gps_rtcm_data_t gps_rtcm_data_msg; + mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg); + + gps_inject_data_s gps_inject_data_topic{}; + + gps_inject_data_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data), + (int)sizeof(uint8_t) * gps_rtcm_data_msg.len); + gps_inject_data_topic.flags = gps_rtcm_data_msg.flags; + memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data, + math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len)); + + _gps_inject_data_pub.publish(gps_inject_data_topic); +} + +void +MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) +{ + mavlink_hil_state_quaternion_t hil_state; + mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); + + const uint64_t timestamp_sample = hrt_absolute_time(); + + /* airspeed */ + { + airspeed_s airspeed{}; + airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; + airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; + airspeed.air_temperature_celsius = 15.f; + airspeed.timestamp = hrt_absolute_time(); + _airspeed_pub.publish(airspeed); + } + + /* attitude */ + { + vehicle_attitude_s hil_attitude{}; + hil_attitude.timestamp_sample = timestamp_sample; + matrix::Quatf q(hil_state.attitude_quaternion); + q.copyTo(hil_attitude.q); + hil_attitude.timestamp = hrt_absolute_time(); + _attitude_pub.publish(hil_attitude); + } + + /* global position */ + { + vehicle_global_position_s hil_global_pos{}; + + hil_global_pos.timestamp_sample = timestamp_sample; + hil_global_pos.lat = hil_state.lat / ((double)1e7); + hil_global_pos.lon = hil_state.lon / ((double)1e7); + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.eph = 2.f; + hil_global_pos.epv = 4.f; + hil_global_pos.timestamp = hrt_absolute_time(); + _global_pos_pub.publish(hil_global_pos); + } + + /* local position */ + { + const double lat = hil_state.lat * 1e-7; + const double lon = hil_state.lon * 1e-7; + + if (!map_projection_initialized(&_global_local_proj_ref) || !PX4_ISFINITE(_global_local_alt0)) { + map_projection_init(&_global_local_proj_ref, lat, lon); + _global_local_alt0 = hil_state.alt / 1000.f; + } + + float x = 0.f; + float y = 0.f; + map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y); + + vehicle_local_position_s hil_local_pos{}; + hil_local_pos.timestamp_sample = timestamp_sample; + hil_local_pos.ref_timestamp = _global_local_proj_ref.timestamp; + hil_local_pos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad); + hil_local_pos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad); + hil_local_pos.ref_alt = _global_local_alt0; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = _global_local_alt0 - hil_state.alt / 1000.f; + hil_local_pos.vx = hil_state.vx / 100.f; + hil_local_pos.vy = hil_state.vy / 100.f; + hil_local_pos.vz = hil_state.vz / 100.f; + + matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)}; + hil_local_pos.heading = euler.psi(); + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + hil_local_pos.vxy_max = INFINITY; + hil_local_pos.vz_max = INFINITY; + hil_local_pos.hagl_min = INFINITY; + hil_local_pos.hagl_max = INFINITY; + hil_local_pos.timestamp = hrt_absolute_time(); + _local_pos_pub.publish(hil_local_pos); + } + + /* accelerometer */ + { + if (_px4_accel == nullptr) { + // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_accel = new PX4Accelerometer(1310988); + + if (_px4_accel == nullptr) { + PX4_ERR("PX4Accelerometer alloc failed"); + } + } + + if (_px4_accel != nullptr) { + // accel in mG + _px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f); + _px4_accel->update(timestamp_sample, hil_state.xacc, hil_state.yacc, hil_state.zacc); + } + } + + /* gyroscope */ + { + if (_px4_gyro == nullptr) { + // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_gyro = new PX4Gyroscope(1310988); + + if (_px4_gyro == nullptr) { + PX4_ERR("PX4Gyroscope alloc failed"); + } + } + + if (_px4_gyro != nullptr) { + _px4_gyro->update(timestamp_sample, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed); + } + } + + /* battery status */ + { + battery_status_s hil_battery_status{}; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; + hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; + hil_battery_status.timestamp = hrt_absolute_time(); + _battery_pub.publish(hil_battery_status); + } +} + +#if !defined(CONSTRAINED_FLASH) +void +MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg) +{ + mavlink_named_value_float_t debug_msg; + mavlink_msg_named_value_float_decode(msg, &debug_msg); + + debug_key_value_s debug_topic{}; + + debug_topic.timestamp = hrt_absolute_time(); + memcpy(debug_topic.key, debug_msg.name, sizeof(debug_topic.key)); + debug_topic.key[sizeof(debug_topic.key) - 1] = '\0'; // enforce null termination + debug_topic.value = debug_msg.value; + + _debug_key_value_pub.publish(debug_topic); +} + +void +MavlinkReceiver::handle_message_debug(mavlink_message_t *msg) +{ + mavlink_debug_t debug_msg; + mavlink_msg_debug_decode(msg, &debug_msg); + + debug_value_s debug_topic{}; + + debug_topic.timestamp = hrt_absolute_time(); + debug_topic.ind = debug_msg.ind; + debug_topic.value = debug_msg.value; + + _debug_value_pub.publish(debug_topic); +} + +void +MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg) +{ + mavlink_debug_vect_t debug_msg; + mavlink_msg_debug_vect_decode(msg, &debug_msg); + + debug_vect_s debug_topic{}; + + debug_topic.timestamp = hrt_absolute_time(); + memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name)); + debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination + debug_topic.x = debug_msg.x; + debug_topic.y = debug_msg.y; + debug_topic.z = debug_msg.z; + + _debug_vect_pub.publish(debug_topic); +} + +void +MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg) +{ + mavlink_debug_float_array_t debug_msg; + mavlink_msg_debug_float_array_decode(msg, &debug_msg); + + debug_array_s debug_topic{}; + + debug_topic.timestamp = hrt_absolute_time(); + debug_topic.id = debug_msg.array_id; + memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name)); + debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination + + for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) { + debug_topic.data[i] = debug_msg.data[i]; + } + + _debug_array_pub.publish(debug_topic); +} +#endif // !CONSTRAINED_FLASH + +void +MavlinkReceiver::handle_message_onboard_computer_status(mavlink_message_t *msg) +{ + mavlink_onboard_computer_status_t status_msg; + mavlink_msg_onboard_computer_status_decode(msg, &status_msg); + + onboard_computer_status_s onboard_computer_status_topic{}; + + onboard_computer_status_topic.timestamp = hrt_absolute_time(); + onboard_computer_status_topic.uptime = status_msg.uptime; + + onboard_computer_status_topic.type = status_msg.type; + + memcpy(onboard_computer_status_topic.cpu_cores, status_msg.cpu_cores, sizeof(status_msg.cpu_cores)); + memcpy(onboard_computer_status_topic.cpu_combined, status_msg.cpu_combined, sizeof(status_msg.cpu_combined)); + memcpy(onboard_computer_status_topic.gpu_cores, status_msg.gpu_cores, sizeof(status_msg.gpu_cores)); + memcpy(onboard_computer_status_topic.gpu_combined, status_msg.gpu_combined, sizeof(status_msg.gpu_combined)); + onboard_computer_status_topic.temperature_board = status_msg.temperature_board; + memcpy(onboard_computer_status_topic.temperature_core, status_msg.temperature_core, + sizeof(status_msg.temperature_core)); + memcpy(onboard_computer_status_topic.fan_speed, status_msg.fan_speed, sizeof(status_msg.fan_speed)); + onboard_computer_status_topic.ram_usage = status_msg.ram_usage; + onboard_computer_status_topic.ram_total = status_msg.ram_total; + memcpy(onboard_computer_status_topic.storage_type, status_msg.storage_type, sizeof(status_msg.storage_type)); + memcpy(onboard_computer_status_topic.storage_usage, status_msg.storage_usage, sizeof(status_msg.storage_usage)); + memcpy(onboard_computer_status_topic.storage_total, status_msg.storage_total, sizeof(status_msg.storage_total)); + memcpy(onboard_computer_status_topic.link_type, status_msg.link_type, sizeof(status_msg.link_type)); + memcpy(onboard_computer_status_topic.link_tx_rate, status_msg.link_tx_rate, sizeof(status_msg.link_tx_rate)); + memcpy(onboard_computer_status_topic.link_rx_rate, status_msg.link_rx_rate, sizeof(status_msg.link_rx_rate)); + memcpy(onboard_computer_status_topic.link_tx_max, status_msg.link_tx_max, sizeof(status_msg.link_tx_max)); + memcpy(onboard_computer_status_topic.link_rx_max, status_msg.link_rx_max, sizeof(status_msg.link_rx_max)); + + _onboard_computer_status_pub.publish(onboard_computer_status_topic); +} + +void MavlinkReceiver::handle_message_generator_status(mavlink_message_t *msg) +{ + mavlink_generator_status_t status_msg; + mavlink_msg_generator_status_decode(msg, &status_msg); + + generator_status_s generator_status{}; + generator_status.timestamp = hrt_absolute_time(); + generator_status.status = status_msg.status; + generator_status.battery_current = status_msg.battery_current; + generator_status.load_current = status_msg.load_current; + generator_status.power_generated = status_msg.power_generated; + generator_status.bus_voltage = status_msg.bus_voltage; + generator_status.bat_current_setpoint = status_msg.bat_current_setpoint; + generator_status.runtime = status_msg.runtime; + generator_status.time_until_maintenance = status_msg.time_until_maintenance; + generator_status.generator_speed = status_msg.generator_speed; + generator_status.rectifier_temperature = status_msg.rectifier_temperature; + generator_status.generator_temperature = status_msg.generator_temperature; + + _generator_status_pub.publish(generator_status); +} + +void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg) +{ + if (msg->sysid == mavlink_system.sysid) { + // log message from the same system + + mavlink_statustext_t statustext; + mavlink_msg_statustext_decode(msg, &statustext); + + log_message_s log_message{}; + + log_message.severity = statustext.severity; + log_message.timestamp = hrt_absolute_time(); + + snprintf(log_message.text, sizeof(log_message.text), + "[mavlink: component %" PRIu8 "] %." STRINGIFY(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN) "s", msg->compid, + statustext.text); + + _log_message_pub.publish(log_message); + } +} + +void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) +{ + // check HEARTBEATs for timeout + static constexpr uint64_t TIMEOUT = telemetry_status_s::HEARTBEAT_TIMEOUT_US; + + if (t <= TIMEOUT) { + return; + } + + if ((t >= _last_heartbeat_check + (TIMEOUT / 2)) || force) { + telemetry_status_s &tstatus = _mavlink->telemetry_status(); + + tstatus.heartbeat_type_antenna_tracker = (t <= TIMEOUT + _heartbeat_type_antenna_tracker); + tstatus.heartbeat_type_gcs = (t <= TIMEOUT + _heartbeat_type_gcs); + tstatus.heartbeat_type_onboard_controller = (t <= TIMEOUT + _heartbeat_type_onboard_controller); + tstatus.heartbeat_type_gimbal = (t <= TIMEOUT + _heartbeat_type_gimbal); + tstatus.heartbeat_type_adsb = (t <= TIMEOUT + _heartbeat_type_adsb); + tstatus.heartbeat_type_camera = (t <= TIMEOUT + _heartbeat_type_camera); + + tstatus.heartbeat_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio); + tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log); + tstatus.heartbeat_component_osd = (t <= TIMEOUT + _heartbeat_component_osd); + tstatus.heartbeat_component_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_component_obstacle_avoidance); + tstatus.heartbeat_component_vio = (t <= TIMEOUT + _heartbeat_component_visual_inertial_odometry); + tstatus.heartbeat_component_pairing_manager = (t <= TIMEOUT + _heartbeat_component_pairing_manager); + tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge); + tstatus.heartbeat_component_uart_bridge = (t <= TIMEOUT + _heartbeat_component_uart_bridge); + + _mavlink->telemetry_status_updated(); + _last_heartbeat_check = t; + } +} + +void +MavlinkReceiver::handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg) +{ + mavlink_gimbal_manager_set_manual_control_t set_manual_control_msg; + mavlink_msg_gimbal_manager_set_manual_control_decode(msg, &set_manual_control_msg); + + gimbal_manager_set_manual_control_s set_manual_control{}; + set_manual_control.timestamp = hrt_absolute_time(); + set_manual_control.origin_sysid = msg->sysid; + set_manual_control.origin_compid = msg->compid; + set_manual_control.target_system = set_manual_control_msg.target_system; + set_manual_control.target_component = set_manual_control_msg.target_component; + set_manual_control.flags = set_manual_control_msg.flags; + set_manual_control.gimbal_device_id = set_manual_control_msg.gimbal_device_id; + + set_manual_control.pitch = set_manual_control_msg.pitch; + set_manual_control.yaw = set_manual_control_msg.yaw; + set_manual_control.pitch_rate = set_manual_control_msg.pitch_rate; + set_manual_control.yaw_rate = set_manual_control_msg.yaw_rate; + + _gimbal_manager_set_manual_control_pub.publish(set_manual_control); +} + +void +MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg) +{ + mavlink_gimbal_manager_set_attitude_t set_attitude_msg; + mavlink_msg_gimbal_manager_set_attitude_decode(msg, &set_attitude_msg); + + gimbal_manager_set_attitude_s gimbal_attitude{}; + gimbal_attitude.timestamp = hrt_absolute_time(); + gimbal_attitude.origin_sysid = msg->sysid; + gimbal_attitude.origin_compid = msg->compid; + gimbal_attitude.target_system = set_attitude_msg.target_system; + gimbal_attitude.target_component = set_attitude_msg.target_component; + gimbal_attitude.flags = set_attitude_msg.flags; + gimbal_attitude.gimbal_device_id = set_attitude_msg.gimbal_device_id; + + matrix::Quatf q(set_attitude_msg.q); + q.copyTo(gimbal_attitude.q); + + gimbal_attitude.angular_velocity_x = set_attitude_msg.angular_velocity_x; + gimbal_attitude.angular_velocity_y = set_attitude_msg.angular_velocity_y; + gimbal_attitude.angular_velocity_z = set_attitude_msg.angular_velocity_z; + + _gimbal_manager_set_attitude_pub.publish(gimbal_attitude); +} + +void +MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg) +{ + + mavlink_gimbal_device_information_t gimbal_device_info_msg; + mavlink_msg_gimbal_device_information_decode(msg, &gimbal_device_info_msg); + + gimbal_device_information_s gimbal_information{}; + gimbal_information.timestamp = hrt_absolute_time(); + + static_assert(sizeof(gimbal_information.vendor_name) == sizeof(gimbal_device_info_msg.vendor_name), + "vendor_name length doesn't match"); + static_assert(sizeof(gimbal_information.model_name) == sizeof(gimbal_device_info_msg.model_name), + "model_name length doesn't match"); + static_assert(sizeof(gimbal_information.custom_name) == sizeof(gimbal_device_info_msg.custom_name), + "custom_name length doesn't match"); + memcpy(gimbal_information.vendor_name, gimbal_device_info_msg.vendor_name, sizeof(gimbal_information.vendor_name)); + memcpy(gimbal_information.model_name, gimbal_device_info_msg.model_name, sizeof(gimbal_information.model_name)); + memcpy(gimbal_information.custom_name, gimbal_device_info_msg.custom_name, sizeof(gimbal_information.custom_name)); + gimbal_device_info_msg.vendor_name[sizeof(gimbal_device_info_msg.vendor_name) - 1] = '\0'; + gimbal_device_info_msg.model_name[sizeof(gimbal_device_info_msg.model_name) - 1] = '\0'; + gimbal_device_info_msg.custom_name[sizeof(gimbal_device_info_msg.custom_name) - 1] = '\0'; + + gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version; + gimbal_information.hardware_version = gimbal_device_info_msg.hardware_version; + gimbal_information.cap_flags = gimbal_device_info_msg.cap_flags; + gimbal_information.custom_cap_flags = gimbal_device_info_msg.custom_cap_flags; + gimbal_information.uid = gimbal_device_info_msg.uid; + + gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max; + gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min; + + gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; + gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; + + gimbal_information.gimbal_device_compid = msg->compid; + + _gimbal_device_information_pub.publish(gimbal_information); +} + +void +MavlinkReceiver::run() +{ + /* set thread name */ + { + char thread_name[17]; + snprintf(thread_name, sizeof(thread_name), "mavlink_rcv_if%d", _mavlink->get_instance_id()); + px4_prctl(PR_SET_NAME, thread_name, px4_getpid()); + } + + // make sure mavlink app has booted before we start processing anything (parameter sync, etc) + while (!_mavlink->boot_complete()) { + if (hrt_elapsed_time(&_mavlink->get_first_start_time()) > 20_s) { + PX4_ERR("system boot did not complete in 20 seconds"); + _mavlink->set_boot_complete(); + } + + px4_usleep(100000); + } + + // poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc. + const int timeout = 10; + +#if defined(__PX4_POSIX) + /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ + uint8_t buf[1600 * 5]; +#elif defined(CONFIG_NET) + /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ + uint8_t buf[1000]; +#else + /* the serial port buffers internally as well, we just need to fit a small chunk */ + uint8_t buf[64]; +#endif + mavlink_message_t msg; + + struct pollfd fds[1] = {}; + + if (_mavlink->get_protocol() == Protocol::SERIAL) { + fds[0].fd = _mavlink->get_uart_fd(); + fds[0].events = POLLIN; + } + +#if defined(MAVLINK_UDP) + struct sockaddr_in srcaddr = {}; + socklen_t addrlen = sizeof(srcaddr); + + if (_mavlink->get_protocol() == Protocol::UDP) { + fds[0].fd = _mavlink->get_socket_fd(); + fds[0].events = POLLIN; + } + +#endif // MAVLINK_UDP + + ssize_t nread = 0; + hrt_abstime last_send_update = 0; + + while (!_mavlink->_task_should_exit) { + + // 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(); + } + + int ret = poll(&fds[0], 1, timeout); + + if (ret > 0) { + if (_mavlink->get_protocol() == Protocol::SERIAL) { + /* non-blocking read. read may return negative values */ + nread = ::read(fds[0].fd, buf, sizeof(buf)); + + if (nread == -1 && errno == ENOTCONN) { // Not connected (can happen for USB) + usleep(100000); + } + } + +#if defined(MAVLINK_UDP) + + else if (_mavlink->get_protocol() == Protocol::UDP) { + if (fds[0].revents & POLLIN) { + nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); + } + + struct sockaddr_in &srcaddr_last = _mavlink->get_client_source_address(); + + int localhost = (127 << 24) + 1; + + if (!_mavlink->get_client_source_initialized()) { + + // set the address either if localhost or if 3 seconds have passed + // this ensures that a GCS running on localhost can get a hold of + // the system within the first N seconds + hrt_abstime stime = _mavlink->get_start_time(); + + if ((stime != 0 && (hrt_elapsed_time(&stime) > 3_s)) + || (srcaddr_last.sin_addr.s_addr == htonl(localhost))) { + + srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr; + srcaddr_last.sin_port = srcaddr.sin_port; + + _mavlink->set_client_source_initialized(); + + PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr)); + } + } + } + + // only start accepting messages on UDP once we're sure who we talk to + if (_mavlink->get_protocol() != Protocol::UDP || _mavlink->get_client_source_initialized()) { +#endif // MAVLINK_UDP + + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &_status)) { + + /* check if we received version 2 and request a switch. */ + if (!(_mavlink->get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) { + /* this will only switch to proto version 2 if allowed in settings */ + _mavlink->set_proto_version(2); + } + + /* handle generic messages and commands */ + handle_message(&msg); + + /* handle packet with mission manager */ + _mission_manager.handle_message(&msg); + + + /* handle packet with parameter component */ + _parameters_manager.handle_message(&msg); + + if (_mavlink->ftp_enabled()) { + /* handle packet with ftp component */ + _mavlink_ftp.handle_message(&msg); + } + + /* handle packet with log component */ + _mavlink_log_handler.handle_message(&msg); + + /* handle packet with timesync component */ + _mavlink_timesync.handle_message(&msg); + + /* handle packet with parent object */ + _mavlink->handle_message(&msg); + + update_rx_stats(msg); + } + } + + /* count received bytes (nread will be -1 on read error) */ + if (nread > 0) { + _mavlink->count_rxbytes(nread); + + telemetry_status_s &tstatus = _mavlink->telemetry_status(); + tstatus.rx_message_count = _total_received_counter; + tstatus.rx_message_lost_count = _total_lost_counter; + tstatus.rx_message_lost_rate = static_cast(_total_lost_counter) / static_cast(_total_received_counter); + + if (_mavlink_status_last_buffer_overrun != _status.buffer_overrun) { + tstatus.rx_buffer_overruns++; + _mavlink_status_last_buffer_overrun = _status.buffer_overrun; + } + + if (_mavlink_status_last_parse_error != _status.parse_error) { + tstatus.rx_parse_errors++; + _mavlink_status_last_parse_error = _status.parse_error; + } + + if (_mavlink_status_last_packet_rx_drop_count != _status.packet_rx_drop_count) { + tstatus.rx_packet_drop_count++; + _mavlink_status_last_packet_rx_drop_count = _status.packet_rx_drop_count; + } + } + +#if defined(MAVLINK_UDP) + } + +#endif // MAVLINK_UDP + + } else if (ret == -1) { + usleep(10000); + } + + const hrt_abstime t = hrt_absolute_time(); + + CheckHeartbeats(t); + + if (t - last_send_update > timeout * 1000) { + _mission_manager.check_active_mission(); + _mission_manager.send(); + + _parameters_manager.send(); + + if (_mavlink->ftp_enabled()) { + _mavlink_ftp.send(); + } + + _mavlink_log_handler.send(); + last_send_update = t; + } + + if (_tune_publisher != nullptr) { + _tune_publisher->publish_next_tune(t); + } + } +} + +void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message) +{ + const bool component_states_has_still_space = [this, &message]() { + for (unsigned i = 0; i < MAX_REMOTE_COMPONENTS; ++i) { + if (_component_states[i].system_id == message.sysid && _component_states[i].component_id == message.compid) { + + int lost_messages = 0; + const uint8_t expected_seq = _component_states[i].last_sequence + 1; + + // Account for overflow during packet loss + if (message.seq < expected_seq) { + lost_messages = (message.seq + 255) - expected_seq; + + } else { + lost_messages = message.seq - expected_seq; + } + + _component_states[i].missed_messages += lost_messages; + + ++_component_states[i].received_messages; + _component_states[i].last_time_received_ms = hrt_absolute_time() / 1000; + _component_states[i].last_sequence = message.seq; + + // Also update overall stats + ++_total_received_counter; + _total_lost_counter += lost_messages; + + return true; + + } else if (_component_states[i].system_id == 0 && _component_states[i].component_id == 0) { + _component_states[i].system_id = message.sysid; + _component_states[i].component_id = message.compid; + + ++_component_states[i].received_messages; + _component_states[i].last_time_received_ms = hrt_absolute_time() / 1000; + _component_states[i].last_sequence = message.seq; + + // Also update overall stats + ++_total_received_counter; + + return true; + } + } + + return false; + }(); + + if (!component_states_has_still_space && !_warned_component_states_full_once) { + PX4_WARN("Max remote components of %u used up", MAX_REMOTE_COMPONENTS); + _warned_component_states_full_once = true; + } +} + +void MavlinkReceiver::print_detailed_rx_stats() const +{ + const uint32_t now_ms = hrt_absolute_time() / 1000; + + // TODO: add mutex around shared data. + for (unsigned i = 0; i < MAX_REMOTE_COMPONENTS; ++i) { + if (_component_states[i].received_messages > 0) { + printf("\t received from sysid: %" PRIu8 " compid: %" PRIu8 ": %" PRIu32 ", lost: %" PRIu32 ", last %" PRIu32 + " ms ago\n", + _component_states[i].system_id, + _component_states[i].component_id, + _component_states[i].received_messages, + _component_states[i].missed_messages, + now_ms - _component_states[i].last_time_received_ms); + + } else { + break; + } + } +} + +void MavlinkReceiver::start() +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + + struct sched_param param; + (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 80; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + + pthread_attr_setstacksize(&receiveloop_attr, + PX4_STACK_ADJUSTED(sizeof(MavlinkReceiver) + 2840 + MAVLINK_RECEIVER_NET_ADDED_STACK)); + + pthread_create(&_thread, &receiveloop_attr, MavlinkReceiver::start_trampoline, (void *)this); + + pthread_attr_destroy(&receiveloop_attr); +} + +void *MavlinkReceiver::start_trampoline(void *context) +{ + MavlinkReceiver *self = reinterpret_cast(context); + self->run(); + return nullptr; +} + +void MavlinkReceiver::stop() +{ + _should_exit.store(true); + pthread_join(_thread, nullptr); +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_receiver.h b/src/prometheus_px4/src/modules/mavlink/mavlink_receiver.h new file mode 100644 index 0000000..4ac7e5a --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_receiver.h @@ -0,0 +1,384 @@ +/**************************************************************************** + * + * 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 mavlink_receiver.h + * MAVLink receiver thread + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +#pragma once + +#include "mavlink_ftp.h" +#include "mavlink_log_handler.h" +#include "mavlink_mission.h" +#include "mavlink_parameters.h" +#include "mavlink_timesync.h" +#include "tune_publisher.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if !defined(CONSTRAINED_FLASH) +# include +# include +# include +# include +#endif // !CONSTRAINED_FLASH + +using namespace time_literals; + +class Mavlink; + +class MavlinkReceiver : public ModuleParams +{ +public: + MavlinkReceiver(Mavlink *parent); + ~MavlinkReceiver() override; + + void start(); + void stop(); + + void print_detailed_rx_stats() const; + +private: + static void *start_trampoline(void *context); + void run(); + + void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result); + + /** + * Common method to handle both mavlink command types. T is one of mavlink_command_int_t or mavlink_command_long_t. + */ + template + void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink, + const vehicle_command_s &vehicle_command); + + uint8_t handle_request_message_command(uint16_t message_id, float param2 = 0.0f, float param3 = 0.0f, + float param4 = 0.0f, + float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); + + void handle_message(mavlink_message_t *msg); + + void handle_message_adsb_vehicle(mavlink_message_t *msg); + void handle_message_att_pos_mocap(mavlink_message_t *msg); + void handle_message_battery_status(mavlink_message_t *msg); + void handle_message_cellular_status(mavlink_message_t *msg); + void handle_message_collision(mavlink_message_t *msg); + void handle_message_command_ack(mavlink_message_t *msg); + void handle_message_command_int(mavlink_message_t *msg); + void handle_message_command_long(mavlink_message_t *msg); + void handle_message_distance_sensor(mavlink_message_t *msg); + void handle_message_follow_target(mavlink_message_t *msg); + void handle_message_generator_status(mavlink_message_t *msg); + void handle_message_set_gps_global_origin(mavlink_message_t *msg); + void handle_message_gps_rtcm_data(mavlink_message_t *msg); + void handle_message_heartbeat(mavlink_message_t *msg); + void handle_message_hil_gps(mavlink_message_t *msg); + void handle_message_hil_optical_flow(mavlink_message_t *msg); + void handle_message_hil_sensor(mavlink_message_t *msg); + void handle_message_hil_state_quaternion(mavlink_message_t *msg); + void handle_message_landing_target(mavlink_message_t *msg); + void handle_message_logging_ack(mavlink_message_t *msg); + void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_obstacle_distance(mavlink_message_t *msg); + void handle_message_odometry(mavlink_message_t *msg); + void handle_message_onboard_computer_status(mavlink_message_t *msg); + void handle_message_optical_flow_rad(mavlink_message_t *msg); + void handle_message_ping(mavlink_message_t *msg); + void handle_message_play_tune(mavlink_message_t *msg); + void handle_message_play_tune_v2(mavlink_message_t *msg); + void handle_message_radio_status(mavlink_message_t *msg); + void handle_message_rc_channels_override(mavlink_message_t *msg); + void handle_message_serial_control(mavlink_message_t *msg); + void handle_message_set_actuator_control_target(mavlink_message_t *msg); + void handle_message_set_attitude_target(mavlink_message_t *msg); + void handle_message_set_mode(mavlink_message_t *msg); + void handle_message_set_position_target_global_int(mavlink_message_t *msg); + void handle_message_set_position_target_local_ned(mavlink_message_t *msg); + void handle_message_statustext(mavlink_message_t *msg); + void handle_message_trajectory_representation_bezier(mavlink_message_t *msg); + void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); + void handle_message_utm_global_position(mavlink_message_t *msg); + void handle_message_vision_position_estimate(mavlink_message_t *msg); + void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg); + void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg); + void handle_message_gimbal_device_information(mavlink_message_t *msg); + +#if !defined(CONSTRAINED_FLASH) + void handle_message_debug(mavlink_message_t *msg); + void handle_message_debug_float_array(mavlink_message_t *msg); + void handle_message_debug_vect(mavlink_message_t *msg); + void handle_message_named_value_float(mavlink_message_t *msg); +#endif // !CONSTRAINED_FLASH + + void CheckHeartbeats(const hrt_abstime &t, bool force = false); + + /** + * Set the interval at which the given message stream is published. + * The rate is the number of messages per second. + * + * @param msgId The ID of the message interval to be set. + * @param interval The interval in usec to send the message. + * @param data_rate The total link data rate in bytes per second. + * + * @return PX4_OK on success, PX4_ERROR on fail. + */ + int set_message_interval(int msgId, float interval, int data_rate = -1); + void get_message_interval(int msgId); + + /** + * Decode a switch position from a bitfield and state. + */ + int decode_switch_pos_n(uint16_t buttons, unsigned sw); + + bool evaluate_target_ok(int command, int target_system, int target_component); + + void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust); + + void schedule_tune(const char *tune); + + void update_rx_stats(const mavlink_message_t &message); + + px4::atomic_bool _should_exit{false}; + pthread_t _thread {}; + + Mavlink *_mavlink; + + MavlinkFTP _mavlink_ftp; + MavlinkLogHandler _mavlink_log_handler; + MavlinkMissionManager _mission_manager; + MavlinkParametersManager _parameters_manager; + MavlinkTimesync _mavlink_timesync; + + mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char() + + orb_advert_t _mavlink_log_pub{nullptr}; + + static constexpr int MAX_REMOTE_COMPONENTS{8}; + struct ComponentState { + uint32_t last_time_received_ms{0}; + uint32_t received_messages{0}; + uint32_t missed_messages{0}; + uint8_t system_id{0}; + uint8_t component_id{0}; + uint8_t last_sequence{0}; + }; + ComponentState _component_states[MAX_REMOTE_COMPONENTS] {}; + bool _warned_component_states_full_once{false}; + + uint64_t _total_received_counter{0}; ///< The total number of successfully received messages + uint64_t _total_lost_counter{0}; ///< Total messages lost during transmission. + + uint8_t _mavlink_status_last_buffer_overrun{0}; + uint8_t _mavlink_status_last_parse_error{0}; + uint16_t _mavlink_status_last_packet_rx_drop_count{0}; + + // ORB publications + uORB::Publication _actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)}; + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + uORB::Publication _battery_pub{ORB_ID(battery_status)}; + uORB::Publication _cellular_status_pub{ORB_ID(cellular_status)}; + uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)}; + uORB::Publication _gimbal_device_information_pub{ORB_ID(gimbal_device_information)}; + uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; + uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; + uORB::Publication _log_message_pub{ORB_ID(log_message)}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; + uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; + uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; + uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; + uORB::Publication _flow_pub{ORB_ID(optical_flow)}; + uORB::Publication _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)}; + uORB::Publication _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)}; + uORB::Publication _global_pos_pub{ORB_ID(vehicle_global_position)}; + uORB::Publication _land_detector_pub{ORB_ID(vehicle_land_detected)}; + uORB::Publication _local_pos_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; + uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; + uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; + uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; + +#if !defined(CONSTRAINED_FLASH) + uORB::Publication _debug_array_pub {ORB_ID(debug_array)}; + uORB::Publication _debug_key_value_pub{ORB_ID(debug_key_value)}; + uORB::Publication _debug_value_pub{ORB_ID(debug_value)}; + uORB::Publication _debug_vect_pub{ORB_ID(debug_vect)}; +#endif // !CONSTRAINED_FLASH + + // ORB publications (multi) + uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; + uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor)}; + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + uORB::PublicationMulti _ping_pub{ORB_ID(ping)}; + uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; + + // ORB publications (queue length > 1) + uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::Publication _transponder_report_pub{ORB_ID(transponder_report)}; + uORB::Publication _cmd_pub{ORB_ID(vehicle_command)}; + uORB::Publication _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + + // ORB subscriptions + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + // hil_sensor and hil_state_quaternion + enum SensorSource { + ACCEL = 0b111, + GYRO = 0b111000, + MAG = 0b111000000, + BARO = 0b1101000000000, + DIFF_PRESS = 0b10000000000 + }; + PX4Accelerometer *_px4_accel{nullptr}; + PX4Barometer *_px4_baro{nullptr}; + PX4Gyroscope *_px4_gyro{nullptr}; + PX4Magnetometer *_px4_mag{nullptr}; + + static constexpr unsigned int MOM_SWITCH_COUNT{8}; + uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; + uint16_t _mom_switch_state{0}; + + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + + hrt_abstime _last_utm_global_pos_com{0}; + + // Allocated if needed. + TunePublisher *_tune_publisher{nullptr}; + + hrt_abstime _last_heartbeat_check{0}; + + hrt_abstime _heartbeat_type_antenna_tracker{0}; + hrt_abstime _heartbeat_type_gcs{0}; + hrt_abstime _heartbeat_type_onboard_controller{0}; + hrt_abstime _heartbeat_type_gimbal{0}; + hrt_abstime _heartbeat_type_adsb{0}; + hrt_abstime _heartbeat_type_camera{0}; + + hrt_abstime _heartbeat_component_telemetry_radio{0}; + hrt_abstime _heartbeat_component_log{0}; + hrt_abstime _heartbeat_component_osd{0}; + hrt_abstime _heartbeat_component_obstacle_avoidance{0}; + hrt_abstime _heartbeat_component_visual_inertial_odometry{0}; + hrt_abstime _heartbeat_component_pairing_manager{0}; + hrt_abstime _heartbeat_component_udp_bridge{0}; + hrt_abstime _heartbeat_component_uart_bridge{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_bat_crit_thr, + (ParamFloat) _param_bat_emergen_thr, + (ParamFloat) _param_bat_low_thr, + (ParamFloat) _param_sens_flow_maxhgt, + (ParamFloat) _param_sens_flow_maxr, + (ParamFloat) _param_sens_flow_minhgt, + (ParamInt) _param_sens_flow_rot + ); + + // Disallow copy construction and move assignment. + MavlinkReceiver(const MavlinkReceiver &) = delete; + MavlinkReceiver operator=(const MavlinkReceiver &) = delete; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_shell.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_shell.cpp new file mode 100644 index 0000000..6203e0f --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_shell.cpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * Copyright (c) 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 mavlink_shell.cpp + * A shell to be used via MAVLink + * + * @author Beat Küng + */ + +#include "mavlink_shell.h" +#include + +#include +#include +#include + + +#ifdef __PX4_NUTTX +#include +#endif /* __PX4_NUTTX */ + +#ifdef __PX4_CYGWIN +#include +#endif + +MavlinkShell::~MavlinkShell() +{ + //closing the pipes will stop the thread as well + if (_to_shell_fd >= 0) { + PX4_INFO("Stopping mavlink shell"); + close(_to_shell_fd); + } + + if (_from_shell_fd >= 0) { + close(_from_shell_fd); + } +} + +int MavlinkShell::start() +{ + //this currently only works for NuttX +#ifndef __PX4_NUTTX + return -1; +#endif /* __PX4_NUTTX */ + + + PX4_INFO("Starting mavlink shell"); + + int p1[2], p2[2]; + + /* Create the shell task and redirect its stdin & stdout. If we used pthread, we would redirect + * stdin/out of the calling process as well, so we need px4_task_spawn_cmd. However NuttX only + * keeps (duplicates) the first 3 fd's when creating a new task, all others are not inherited. + * This means we need to temporarily change the first 3 fd's of the current task (or at least + * the first 2 if stdout=stderr). + */ + + if (pipe(p1) != 0) { + return -errno; + } + + if (pipe(p2) != 0) { + close(p1[0]); + close(p1[1]); + return -errno; + } + + int ret = 0; + + _from_shell_fd = p1[0]; + _to_shell_fd = p2[1]; + _shell_fds[0] = p2[0]; + _shell_fds[1] = p1[1]; + + /* + * Ensure that during the temporary phase no other thread from the same task writes to + * stdout (as it would end up in the pipe). + */ +#ifdef __PX4_NUTTX + sched_lock(); +#endif /* __PX4_NUTTX */ + fflush(stdout); + fflush(stderr); + + int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task + + for (int i = 0; i < 2; ++i) { + fd_backups[i] = dup(i); + + if (fd_backups[i] == -1) { + ret = -errno; + } + } + + dup2(_shell_fds[0], 0); + dup2(_shell_fds[1], 1); + + if (ret == 0) { + _task = px4_task_spawn_cmd("mavlink_shell", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + &MavlinkShell::shell_start_thread, + nullptr); + + if (_task < 0) { + ret = -1; + } + } + + //restore fd's + for (int i = 0; i < 2; ++i) { + if (dup2(fd_backups[i], i) == -1) { + ret = -errno; + } + + close(fd_backups[i]); + } + +#ifdef __PX4_NUTTX + sched_unlock(); +#endif /* __PX4_NUTTX */ + + //close unused pipe fd's + close(_shell_fds[0]); + close(_shell_fds[1]); + + return ret; +} + +int MavlinkShell::shell_start_thread(int argc, char *argv[]) +{ + dup2(1, 2); //redirect stderror to stdout + +#ifdef __PX4_NUTTX + nsh_consolemain(0, NULL); +#endif /* __PX4_NUTTX */ + + return 0; +} + +size_t MavlinkShell::write(uint8_t *buffer, size_t len) +{ + return ::write(_to_shell_fd, buffer, len); +} + +size_t MavlinkShell::read(uint8_t *buffer, size_t len) +{ + return ::read(_from_shell_fd, buffer, len); +} + +size_t MavlinkShell::available() +{ + int ret = 0; + + if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) { + return ret; + } + + return 0; +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_shell.h b/src/prometheus_px4/src/modules/mavlink/mavlink_shell.h new file mode 100644 index 0000000..649b429 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_shell.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 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 mavlink_shell.h + * A shell to be used via MAVLink + * + * @author Beat Küng + */ + + +#include +#include +#include + +#pragma once + +class MavlinkShell +{ +public: + MavlinkShell() = default; + + ~MavlinkShell(); + + /** + * Start the mavlink shell. + * + * @return 0 on success. + */ + int start(); + + /** + * Write to the shell + * @return number of written bytes + */ + size_t write(uint8_t *buffer, size_t len); + + /** + * Read from the shell. This is blocking, if 0 bytes are available, this will block. + * @param len buffer length + * @return number of bytes read. + */ + size_t read(uint8_t *buffer, size_t len); + + /** + * Get the number of bytes that can be read. + */ + size_t available(); + +private: + + int _to_shell_fd = -1; /** fd to write to the shell */ + int _from_shell_fd = -1; /** fd to read from the shell */ + int _shell_fds[2] = { -1, -1}; /** stdin & out used by the shell */ + px4_task_t _task; + + static int shell_start_thread(int argc, char *argv[]); + + /* do not allow copying this class */ + MavlinkShell(const MavlinkShell &) = delete; + MavlinkShell operator=(const MavlinkShell &) = delete; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_simple_analyzer.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_simple_analyzer.cpp new file mode 100644 index 0000000..7ec6edf --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_simple_analyzer.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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 mavlink_simple_analyzer.cpp + * + * @author Achermann Florian + */ + +#include "mavlink_simple_analyzer.h" + +#include + +#include +#include + +SimpleAnalyzer::SimpleAnalyzer(Mode mode, float window) : + _window(window), + _mode(mode) +{ + reset(); +} + +void SimpleAnalyzer::reset() +{ + _n = 0; + + switch (_mode) { + case AVERAGE: + _result = 0.0f; + + break; + + case MIN: + _result = FLT_MAX; + + break; + + case MAX: + _result = FLT_MIN; + + break; + + default: + PX4_ERR("SimpleAnalyzer: Unknown mode."); + } +} + +void SimpleAnalyzer::add_value(float val, float update_rate) +{ + switch (_mode) { + case AVERAGE: + _result = (_result * _n + val) / (_n + 1u); + + break; + + case MIN: + if (val < _result) { + _result = val; + } + + break; + + case MAX: + if (val > _result) { + _result = val; + } + + break; + } + + // if we get more measurements than n_max so the exponential moving average + // is computed + if ((_n < update_rate * _window) && (update_rate > 1.0f)) { + _n++; + } + + // value sanity checks + if (!PX4_ISFINITE(_result)) { + PX4_DEBUG("SimpleAnalyzer: Result is not finite, reset the analyzer."); + reset(); + } +} + +bool SimpleAnalyzer::valid() const +{ + return _n > 0u; +} + +float SimpleAnalyzer::get() const +{ + return _result; +} + +float SimpleAnalyzer::get_scaled(float scalingfactor) const +{ + return get() * scalingfactor; +} + +void SimpleAnalyzer::check_limits(float &x, float min, float max) const +{ + if (x > max) { + x = max; + + } else if (x < min) { + x = min; + } +} + +void SimpleAnalyzer::int_round(float &x) const +{ + if (x < 0) { + x -= 0.5f; + + } else { + x += 0.5f; + } +} + +void convert_limit_safe(float in, uint16_t &out) +{ + if (in > UINT16_MAX) { + out = UINT16_MAX; + + } else if (in < 0) { + out = 0; + + } else { + out = static_cast(in); + } +} + +void convert_limit_safe(float in, int16_t &out) +{ + if (in > INT16_MAX) { + out = INT16_MAX; + + } else if (in < INT16_MIN) { + out = INT16_MIN; + + } else { + out = static_cast(in); + } +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_simple_analyzer.h b/src/prometheus_px4/src/modules/mavlink/mavlink_simple_analyzer.h new file mode 100644 index 0000000..d02423f --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_simple_analyzer.h @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 mavlink_simple_analyzer.h + * + * @author Achermann Florian + */ + +#pragma once + +#include +#include + +/** + * SimpleAnalyzer + * + * Class used for simple analysis of data streams. + * The data can be analyzed in the following three modes: + * + * AVERAGE: + * The average value is computed at the beginning. Based on the number of analyzed values, + * the update rate and the window size the switch to the moving average is determined. + * + * MIN: + * The minimum value is tracked. + * + * MAX: + * The maximum value is tracked. + */ +class SimpleAnalyzer +{ +public: + enum Mode { + AVERAGE = 0, + MIN, + MAX, + }; + + /** + * Constructor + * + * Defines the mode of the analyzer and the window size in case of the + * averaging mode. + * + * @param[in] mode: The mode of the analyzer + * @param[in] window: The window size in seconds. Only used in the averaging mode. + */ + SimpleAnalyzer(Mode mode, float window = 60.0f); + + /** + * Reset the analyzer to the initial state. + */ + void reset(); + + /** + * Add a new value to the analyzer and update the result according to the mode. + * + * @param[in] val: The value to process + * @param[in] update_rate: The update rate in [1/s] for which new value are added. + * Used in the averaging mode to determine when to switch from averaging to the moving average. + */ + void add_value(float val, float update_rate); + + /** + * Returns true if at least one value has been added to the analyzer. + */ + bool valid() const; + + /** + * Get the current result of the analyzer. + */ + float get() const; + + /** + * Get the scaled value of the current result of the analyzer. + * + * @param[in] scalingfactor: The factor used to scale the result. + */ + float get_scaled(float scalingfactor) const; + + /** + * Get the rounded scaled value casted to the input template type. + * Should only be used to return integer types. + * + * @param[out] ret: The scaled and rounded value of the current analyzer result. + * @parma[in] scalingfactor: The factor which is used to scale the result. + */ + void get_scaled(uint8_t &ret, float scalingfactor) const + { + float avg = get_scaled(scalingfactor); + int_round(avg); + check_limits(avg, 0, UINT8_MAX); + + ret = static_cast(avg); + } + + void get_scaled(int8_t &ret, float scalingfactor) const + { + float avg = get_scaled(scalingfactor); + int_round(avg); + check_limits(avg, INT8_MIN, INT8_MAX); + + ret = static_cast(avg); + } + +private: + unsigned int _n = 0; /**< The number of added samples */ + float _window = 60.0f; /**< The window size for the moving average filter [s] */ + Mode _mode = AVERAGE; /**< The mode of the simple analyzer */ + float _result = 0.0f; /**< The result of the analyzed data. */ + + void check_limits(float &x, float min, float max) const; + void int_round(float &x) const; +}; + +void convert_limit_safe(float in, uint16_t &out); +void convert_limit_safe(float in, int16_t &out); diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_stream.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_stream.cpp new file mode 100644 index 0000000..6b00fd4 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_stream.cpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 mavlink_stream.cpp + * Mavlink messages stream implementation. + * + * @author Anton Babushkin + * @author Lorenz Meier + */ + +#include + +#include "mavlink_stream.h" +#include "mavlink_main.h" + +MavlinkStream::MavlinkStream(Mavlink *mavlink) : + _mavlink(mavlink) +{ + _last_sent = hrt_absolute_time(); +} + +/** + * Update subscriptions and send message if necessary + */ +int +MavlinkStream::update(const hrt_abstime &t) +{ + update_data(); + + // If the message has never been sent before we want + // to send it immediately and can return right away + if (_last_sent == 0) { + // this will give different messages on the same run a different + // initial timestamp which will help spacing them out + // on the link scheduling + if (send()) { + _last_sent = hrt_absolute_time(); + + if (!_first_message_sent) { + _first_message_sent = true; + } + } + + return 0; + } + + // One of the previous iterations sent the update + // already before the deadline + if (_last_sent > t) { + return -1; + } + + int64_t dt = t - _last_sent; + int interval = _interval; + + if (!const_rate()) { + interval /= _mavlink->get_rate_mult(); + } + + // We don't need to send anything if the inverval is 0. send() will be called manually. + if (interval == 0) { + return 0; + } + + const bool unlimited_rate = interval < 0; + + // Send the message if it is due or + // if it will overrun the next scheduled send interval + // by 30% of the interval time. This helps to avoid + // sending a scheduled message on average slower than + // scheduled. Doing this at 50% would risk sending + // the message too often as the loop runtime of the app + // needs to be accounted for as well. + // This method is not theoretically optimal but a suitable + // stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz) + + if (unlimited_rate || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 3))) { + // interval expired, send message + + // If the interval is non-zero and dt is smaller than 1.5 times the interval + // do not use the actual time but increment at a fixed rate, so that processing delays do not + // distort the average rate. The check of the maximum interval is done to ensure that after a + // long time not sending anything, sending multiple messages in a short time is avoided. + if (send()) { + _last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t; + + if (!_first_message_sent) { + _first_message_sent = true; + } + + return 0; + + } else { + return -1; + } + } + + return -1; +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_stream.h b/src/prometheus_px4/src/modules/mavlink/mavlink_stream.h new file mode 100644 index 0000000..4bb4c11 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_stream.h @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 mavlink_stream.h + * Mavlink messages stream definition. + * + * @author Anton Babushkin + */ + +#ifndef MAVLINK_STREAM_H_ +#define MAVLINK_STREAM_H_ + +#include +#include +#include + +class Mavlink; + +class MavlinkStream : public ListNode +{ + +public: + + MavlinkStream(Mavlink *mavlink); + virtual ~MavlinkStream() = default; + + // no copy, assignment, move, move assignment + MavlinkStream(const MavlinkStream &) = delete; + MavlinkStream &operator=(const MavlinkStream &) = delete; + MavlinkStream(MavlinkStream &&) = delete; + MavlinkStream &operator=(MavlinkStream &&) = delete; + + /** + * Get the interval + * + * @param interval the interval in microseconds (us) between messages + */ + void set_interval(const int interval) { _interval = interval; } + + /** + * Get the interval + * + * @return the inveral in microseconds (us) between messages + */ + int get_interval() { return _interval; } + + /** + * @return 0 if updated / sent, -1 if unchanged + */ + int update(const hrt_abstime &t); + virtual const char *get_name() const = 0; + virtual uint16_t get_id() = 0; + + /** + * @return true if steam rate shouldn't be adjusted + */ + virtual bool const_rate() { return false; } + + /** + * Get maximal total messages size on update + */ + virtual unsigned get_size() = 0; + + /** + * This function is called in response to a MAV_CMD_REQUEST_MESSAGE command. + */ + virtual bool request_message(float param2 = 0.0, float param3 = 0.0, float param4 = 0.0, + float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) + { + return send(); + } + + /** + * Get the average message size + * + * For a normal stream this equals the message size, + * for something like a parameter or mission message + * this equals usually zero, as no bandwidth + * needs to be reserved + */ + virtual unsigned get_size_avg() { return get_size(); } + + /** + * @return true if the first message of this stream has been sent + */ + bool first_message_sent() const { return _first_message_sent; } + + /** + * Reset the time of last sent to 0. Can be used if a message over this + * stream needs to be sent immediately. + */ + void reset_last_sent() { _last_sent = 0; } + +protected: + Mavlink *const _mavlink; + int _interval{1000000}; ///< if set to negative value = unlimited rate + + virtual bool send() = 0; + + /** + * Function to collect/update data for the streams at a high rate independant of + * actual stream rate. + * + * This function is called at every iteration of the mavlink module. + */ + virtual void update_data() { } + +private: + hrt_abstime _last_sent{0}; + bool _first_message_sent{false}; +}; + + +#endif /* MAVLINK_STREAM_H_ */ diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_tests/CMakeLists.txt b/src/prometheus_px4/src/modules/mavlink/mavlink_tests/CMakeLists.txt new file mode 100644 index 0000000..398a818 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_tests/CMakeLists.txt @@ -0,0 +1,53 @@ +############################################################################ +# +# 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. +# +############################################################################ + +include_directories(${PX4_SOURCE_DIR}/mavlink/include/mavlink) + +px4_add_module( + MODULE modules__mavlink__mavlink_tests + MAIN mavlink_tests + STACK_MAIN 5000 + COMPILE_FLAGS + -DMAVLINK_FTP_UNIT_TEST + #-DMAVLINK_FTP_DEBUG + -DMavlinkStream=MavlinkStreamTest + -DMavlinkFTP=MavlinkFTPTest + -Wno-cast-align # TODO: fix and enable + -Wno-address-of-packed-member # TODO: fix in c_library_v2 + -Wno-double-promotion # The fix has been proposed as PR upstream (2020-03-08) + SRCS + mavlink_tests.cpp + mavlink_ftp_test.cpp + ../mavlink_stream.cpp + ../mavlink_ftp.cpp + ) diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp new file mode 100644 index 0000000..77c17e1 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -0,0 +1,1021 @@ +/**************************************************************************** + * + * Copyright (C) 2014-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 mavlink_ftp_test.cpp +/// @author Don Gagne + +#include +#include +#include +#include + +#include "mavlink_ftp_test.h" +#include "../mavlink_ftp.h" + +#ifdef __PX4_NUTTX +#define PX4_MAVLINK_TEST_DATA_DIR "/fs/microsd/ftp_unit_test_data" +#else +#define PX4_MAVLINK_TEST_DATA_DIR "ftp_unit_test_data" +#endif + +static const char *_test_files[] = { + PX4_MAVLINK_TEST_DATA_DIR "/" "test_238.data", + PX4_MAVLINK_TEST_DATA_DIR "/" "test_239.data", + PX4_MAVLINK_TEST_DATA_DIR "/" "test_240.data" +}; + +const MavlinkFtpTest::DownloadTestCase MavlinkFtpTest::_rgDownloadTestCases[] = { + { _test_files[0], MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1, true, false }, // Read takes less than single packet + { _test_files[1], MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader), true, true }, // Read completely fills single packet + { _test_files[2], MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1, false, false }, // Read take two packets +}; + +const char MavlinkFtpTest::_unittest_microsd_dir[] = PX4_STORAGEDIR "/ftp_unit_test_dir"; +const char MavlinkFtpTest::_unittest_microsd_file[] = PX4_STORAGEDIR "/ftp_unit_test_dir/file"; + +MavlinkFtpTest::MavlinkFtpTest() : + _ftp_server(nullptr), + _expected_seq_number(0), + _reply_msg{} +{ +} + +/// @brief Called before every test to initialize the FTP Server. +void MavlinkFtpTest::_init() +{ + _expected_seq_number = 0; + _ftp_server = new MavlinkFTP(nullptr); + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this); + + _create_test_files(); + + _cleanup_microsd(); +} + +bool MavlinkFtpTest::_create_test_files() +{ + int ret = ::mkdir(PX4_MAVLINK_TEST_DATA_DIR, S_IRWXU | S_IRWXG | S_IRWXO); + ut_assert("mkdir failed", ret == 0 || errno == EEXIST); + + ret = ::mkdir(PX4_MAVLINK_TEST_DATA_DIR "/empty_dir", S_IRWXU | S_IRWXG | S_IRWXO); + ut_assert("mkdir failed", ret == 0 || errno == EEXIST); + + bool failed = false; + + for (int i = 0; i < 3; ++i) { + int fd = ::open(_test_files[i], O_CREAT | O_EXCL | O_WRONLY, S_IRWXU | S_IRWXG | S_IRWXO); + + if (fd < 0) { + printf("fd: %d, error: %s\n", fd, strerror(errno)); + ut_assert("Open failed", fd != -1); + } + + // We create 3 files, with bytes counting from 0 to 238, 239, and 240. + uint8_t len = 238 + i; + + for (uint8_t c = 0; c < len; ++c) { + ret = ::write(fd, &c, 1); + + if (ret != 1) { + failed = true; + } + } + + close(fd); + } + + ut_assert("Could not write test file", !failed); + + return !failed; +} + +/// @brief Called after every test to take down the FTP Server. +void MavlinkFtpTest::_cleanup() +{ + delete _ftp_server; + + _cleanup_microsd(); + _remove_test_files(); +} + +bool MavlinkFtpTest::_remove_test_files() +{ + for (int i = 0; i < 3; ++i) { + ::unlink(_test_files[i]); + } + + ::rmdir(PX4_MAVLINK_TEST_DATA_DIR "/empty_dir"); + ::rmdir(PX4_MAVLINK_TEST_DATA_DIR); + + return true; +} + + +/// @brief Tests for correct behavior of an Ack response. +bool MavlinkFtpTest::_ack_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + + return true; +} + +/// @brief Tests for correct response to an invalid opcode. +bool MavlinkFtpTest::_bad_opcode_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + + payload.opcode = 0xFF; // bogus opcode + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand); + + return true; +} + +/// @brief Tests for correct reponse to a payload which an invalid data size field. +bool MavlinkFtpTest::_bad_datasize_test() +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + // Set the data size to be one larger than is legal + ((MavlinkFTP::PayloadHeader *)((mavlink_file_transfer_protocol_t *)msg.payload64)->payload)->size = + MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1; + + _ftp_server->handle_message(&msg); + + if (!_decode_message(&_reply_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize); + + return true; +} + +bool MavlinkFtpTest::_list_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + + struct _testCase { + const char *dir; ///< Directory to run List command on + const char *response; ///< Expected response entries from List command + int response_count; ///< Number of directories that should be returned + bool success; ///< true: List command should succeed, false: List command should fail + }; + struct _testCase rgTestCases[] = { + { "/bogus", nullptr, 0, false }, +#ifdef __PX4_NUTTX + { PX4_MAVLINK_TEST_DATA_DIR, "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240", 4, true }, +#else + { PX4_MAVLINK_TEST_DATA_DIR, "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240|S|S", 6, true }, // readdir on Linux adds . and .. +#endif + }; + + for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) { + const struct _testCase *test = &rgTestCases[i]; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir) + 1, // size in bytes of data + (uint8_t *)test->dir, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1); + + // The return order of directories from the List command is not repeatable. So we can't do a direct comparison + // to a hardcoded return result string. + + // Convert null terminators to seperator char so we can use strok to parse returned data + char list_entry[256]; + + for (uint8_t j = 0; j < reply->size - 1; j++) { + if (reply->data[j] == 0) { + list_entry[j] = '|'; + + } else { + list_entry[j] = reply->data[j]; + } + } + + list_entry[reply->size - 1] = 0; + + // Loop over returned directory entries trying to find then in the response list + char *dir; + int response_count = 0; + dir = strtok(list_entry, "|"); + + while (dir != nullptr) { + ut_assert("Returned directory not found in expected response", strstr(test->response, dir)); + response_count++; + dir = strtok(nullptr, "|"); + } + + ut_compare("Incorrect number of directory entires returned", test->response_count, response_count); + + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFileNotFound); + ut_compare("Incorrect payload size", reply->size, 1); + } + } + + return true; +} + +/// @brief Tests for correct response to a List command on a valid directory, but with an offset that +/// is beyond the last directory entry. +bool MavlinkFtpTest::_list_eof_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + const char *dir = PX4_MAVLINK_TEST_DATA_DIR; + + payload.opcode = MavlinkFTP::kCmdListDirectory; +#ifdef __PX4_NUTTX + payload.offset = 4; // (3 test files, 1 test folder) +#else + payload.offset = 6; // (3 test files, 1 test folder, two skipped ./..) +#endif + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir) + 1, // size in bytes of data + (uint8_t *)dir, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF); + + return true; +} + +/// @brief Tests for correct response to an Open command on a file which does not exist. +bool MavlinkFtpTest::_open_badfile_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + const char *dir = "/foo"; // non-existent file + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir) + 1, // size in bytes of data + (uint8_t *)dir, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFileNotFound); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate +bool MavlinkFtpTest::_open_terminate_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + + for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) { + struct stat st; + const DownloadTestCase *test = &_rgDownloadTestCases[i]; + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file) + 1, // size in bytes of data + (uint8_t *)test->file, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("stat failed", stat(test->file, &st), 0); + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t)); + ut_compare("File size incorrect", *((uint32_t *)&reply->data[0]), (uint32_t)st.st_size); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Terminate command on an invalid session. +bool MavlinkFtpTest::_terminate_badsession_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + const char *file = _rgDownloadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file) + 1, // size in bytes of data + (uint8_t *)file, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session + 1; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an open session. +bool MavlinkFtpTest::_read_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + + for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) { + struct stat st; + const DownloadTestCase *test = &_rgDownloadTestCases[i]; + + // Read in the file so we can compare it to what we get back + ut_compare("stat failed", stat(test->file, &st), 0); + uint8_t *bytes = new uint8_t[st.st_size]; + ut_assert("new failed", bytes != nullptr); + int fd = ::open(test->file, O_RDONLY); + ut_assert("open failed", fd != -1); + int bytes_read = ::read(fd, bytes, st.st_size); + ut_compare("read failed", bytes_read, st.st_size); + ::close(fd); + + // Test case data files are created for specific boundary conditions + ut_compare("Test case data files are out of date", test->length, st.st_size); + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file) + 1, // size in bytes of data + (uint8_t *)test->file, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + delete[] bytes; + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session; + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + delete[] bytes; + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader); + uint32_t expected_bytes = test->singlePacketRead ? (uint32_t)st.st_size : full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("File contents differ", memcmp(reply->data, bytes, expected_bytes), 0); + + payload.offset += expected_bytes; + + if (test->singlePacketRead) { + // Try going past EOF + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + delete[] bytes; + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + + } else { + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + delete[] bytes; + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, payload.offset); + + expected_bytes = (uint32_t)st.st_size - full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("File contents differ", memcmp(reply->data, &bytes[payload.offset], expected_bytes), 0); + } + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + delete[] bytes; + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + + delete[] bytes; + bytes = nullptr; + } + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an open session. +bool MavlinkFtpTest::_burst_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + BurstInfo burst_info; + + + + for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) { + struct stat st; + const DownloadTestCase *test = &_rgDownloadTestCases[i]; + + // Read in the file so we can compare it to what we get back + ut_compare("stat failed", stat(test->file, &st), 0); + uint8_t *bytes = new uint8_t[st.st_size]; + ut_assert("new failed", bytes != nullptr); + int fd = ::open(test->file, O_RDONLY); + ut_assert("open failed", fd != -1); + int bytes_read = ::read(fd, bytes, st.st_size); + ut_compare("read failed", bytes_read, st.st_size); + ::close(fd); + + // Test case data files are created for specific boundary conditions + ut_compare("Test case data files are out of date", test->length, st.st_size); + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file) + 1, // size in bytes of data + (uint8_t *)test->file, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + delete[] bytes; + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + // Setup for burst response handler + burst_info.burst_state = burst_state_first_ack; + burst_info.single_packet_file = test->singlePacketRead; + burst_info.file_size = st.st_size; + burst_info.file_bytes = bytes; + burst_info.ftp_test_class = this; + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_burst, &burst_info); + + // Send the burst command, message response will be handled by _receive_message_handler_stream + payload.opcode = MavlinkFTP::kCmdBurstReadFile; + payload.session = reply->session; + payload.offset = 0; + + mavlink_message_t msg; + _setup_ftp_msg(&payload, 0, nullptr, &msg); + _ftp_server->handle_message(&msg); + + // First packet is sent using stream mechanism, so we need to force it out ourselves + _ftp_server->send(); + + ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete); + + // Put back generic message handler + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this); + + // Terminate session + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + delete[] bytes; + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + + delete[] bytes; + bytes = nullptr; + } + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an invalid session. +bool MavlinkFtpTest::_read_badsession_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + const char *file = _rgDownloadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFileRO; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file) + 1, // size in bytes of data + (uint8_t *)file, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session + 1; // Invalid session + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +bool MavlinkFtpTest::_removedirectory_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *dir; + bool success; + bool deleteFile; + uint8_t reply_size; + uint8_t error_code; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false, false, 1, MavlinkFTP::kErrFileNotFound }, + { _unittest_microsd_dir, false, false, 2, MavlinkFTP::kErrFailErrno }, + { _unittest_microsd_file, false, false, 2, MavlinkFTP::kErrFailErrno }, + { _unittest_microsd_dir, true, true, 0, MavlinkFTP::kErrNone }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL, S_IRWXU | S_IRWXG | S_IRWXO)) != -1); + ::close(fd); + + for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) { + const struct _testCase *test = &rgTestCases[i]; + + if (test->deleteFile) { + ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0); + } + + payload.opcode = MavlinkFTP::kCmdRemoveDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir) + 1, // size in bytes of data + (uint8_t *)test->dir, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, test->reply_size); + + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, test->reply_size); + ut_compare("Incorrect error code", reply->data[0], test->error_code); + } + } + + return true; +} + +bool MavlinkFtpTest::_createdirectory_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + + struct _testCase { + const char *dir; + bool success; + uint8_t reply_size; + uint8_t error_code; + }; + static const struct _testCase rgTestCases[] = { + { _unittest_microsd_dir, true, 0, MavlinkFTP::kErrNone}, + { _unittest_microsd_dir, false, 1, MavlinkFTP::kErrFailFileExists}, +#ifdef __PX4_NUTTX + { PX4_MAVLINK_TEST_DATA_DIR "/bogus/bogus", false, 2, MavlinkFTP::kErrFailErrno} // on NuttX missing folders is EIO +#else + { PX4_MAVLINK_TEST_DATA_DIR "/bogus/bogus", false, 1, MavlinkFTP::kErrFileNotFound} // on Linux it is ENOENT +#endif + }; + + for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) { + const struct _testCase *test = &rgTestCases[i]; + + payload.opcode = MavlinkFTP::kCmdCreateDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir) + 1, // size in bytes of data + (uint8_t *)test->dir, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, test->reply_size); + + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect error code", reply->data[0], test->error_code); + ut_compare("Incorrect payload size", reply->size, test->reply_size); + } + } + + return true; +} + +bool MavlinkFtpTest::_removefile_test() +{ + MavlinkFTP::PayloadHeader payload; + const MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *file; + bool success; + uint8_t reply_size; + uint8_t error_code; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false, 1, MavlinkFTP::kErrFileNotFound }, + { _unittest_microsd_dir, false, 2, MavlinkFTP::kErrFailErrno }, + { _unittest_microsd_file, true, 0, MavlinkFTP::kErrNone }, + { _unittest_microsd_file, false, 1, MavlinkFTP::kErrFileNotFound }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL, S_IRWXU | S_IRWXG | S_IRWXO)) != -1); + ::close(fd); + + for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) { + const struct _testCase *test = &rgTestCases[i]; + + payload.opcode = MavlinkFTP::kCmdRemoveFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file) + 1, // size in bytes of data + (uint8_t *)test->file, // Data to start into FTP message payload + &reply); // Payload inside FTP message response + + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, test->reply_size); + + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, test->reply_size); + ut_compare("Incorrect error code", reply->data[0], test->error_code); + } + } + + return true; +} + +/// Static method used as callback from MavlinkFTP for generic use. This method will be called by MavlinkFTP when +/// it needs to send a message out on Mavlink. +void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data) +{ + ((MavlinkFtpTest *)worker_data)->_receive_message_handler_generic(ftp_req); +} + +void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req) +{ + // Move the message into our own member variable + memcpy(&_reply_msg, ftp_req, sizeof(mavlink_file_transfer_protocol_t)); +} + +/// Static method used as callback from MavlinkFTP for stream download testing. This method will be called by MavlinkFTP when +/// it needs to send a message out on Mavlink. +void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data) +{ + BurstInfo *burst_info = (BurstInfo *)worker_data; + burst_info->ftp_test_class->_receive_message_handler_burst(ftp_req, burst_info); +} + +bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_msg, + BurstInfo *burst_info) +{ + const MavlinkFTP::PayloadHeader *reply{nullptr}; + uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader); + uint32_t expected_bytes; + + _decode_message(ftp_msg, &reply); + + switch (burst_info->burst_state) { + case burst_state_first_ack: + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("burst_complete incorrect", reply->burst_complete, 0); + ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0); + + // Setup for next expected message + burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack; + + ut_assert("Remaining stream packets missing", _ftp_server->get_size()); + _ftp_server->send(); + break; + + case burst_state_last_ack: + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, full_packet_bytes); + + expected_bytes = burst_info->file_size - full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("burst_complete incorrect", reply->burst_complete, 0); + ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0); + + // Setup for next expected message + burst_info->burst_state = burst_state_nak_eof; + + ut_assert("Remaining stream packets missing", _ftp_server->get_size()); + _ftp_server->send(); + break; + + case burst_state_nak_eof: + // Signal complete + burst_info->burst_state = burst_state_complete; + ut_compare("All packets should have been seent", _ftp_server->get_size(), 0); + break; + + } + + return true; +} + +/// @brief Decode and validate the incoming message +bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, ///< Incoming FTP message + const MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response +{ + //warnx("_decode_message"); + + // Make sure the targets are correct + ut_compare("Target network non-zero", ftp_msg->target_network, 0); + ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId); + ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId); + + *payload = reinterpret_cast(ftp_msg->payload); + + // Make sure we have a good sequence number + ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number); + _expected_seq_number++; + + return true; +} + +/// @brief Initializes an FTP message into a mavlink message +void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_message_t *msg) ///< Returned mavlink message +{ + uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN]; + MavlinkFTP::PayloadHeader *payload = reinterpret_cast(payload_bytes); + + memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader)); + + payload->seq_number = _expected_seq_number++; + payload->size = size; + + if (size != 0) { + memcpy(payload->data, data, size); + } + + payload->burst_complete = 0; + payload->padding = 0; + + msg->checksum = 0; + mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id + clientComponentId, // Sender component id + msg, // Message to pack payload into + 0, // Target network + serverSystemId, // Target system id + serverComponentId, // Target component id + payload_bytes); // Payload to pack into message +} + +/// @brief Sends the specified FTP message to the server and returns response +bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + const MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response +{ + mavlink_message_t msg; + + _setup_ftp_msg(payload_header, size, data, &msg); + _ftp_server->handle_message(&msg); + return _decode_message(&_reply_msg, payload_reply); +} + +/// @brief Cleans up an files created on microsd during testing +void MavlinkFtpTest::_cleanup_microsd() +{ + ::unlink(_unittest_microsd_file); + ::rmdir(_unittest_microsd_dir); +} + +/// @brief Runs all the unit tests +bool MavlinkFtpTest::run_tests() +{ + ut_run_test(_ack_test); + ut_run_test(_bad_opcode_test); + ut_run_test(_bad_datasize_test); + ut_run_test(_list_test); + ut_run_test(_list_eof_test); + ut_run_test(_open_badfile_test); + ut_run_test(_open_terminate_test); + ut_run_test(_terminate_badsession_test); + ut_run_test(_read_test); + ut_run_test(_read_badsession_test); + ut_run_test(_burst_test); + ut_run_test(_removedirectory_test); + ut_run_test(_createdirectory_test); + ut_run_test(_removefile_test); + + return (_tests_failed == 0); + +} + +ut_declare_test(mavlink_ftp_test, MavlinkFtpTest) diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h new file mode 100644 index 0000000..8d2868a --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (C) 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 mavlink_ftp_test.h +/// @author Don Gagne + +#pragma once + +#include +#ifndef MAVLINK_FTP_UNIT_TEST +#include "../mavlink_bridge_header.h" +#else +#include +#endif +#include "../mavlink_ftp.h" + +class MavlinkFtpTest : public UnitTest +{ +public: + MavlinkFtpTest(); + virtual ~MavlinkFtpTest() = default; + + virtual bool run_tests(void); + + static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data); + + /// Worker data for stream handler + struct BurstInfo { + MavlinkFtpTest *ftp_test_class; + int burst_state; + bool single_packet_file; + uint32_t file_size; + uint8_t *file_bytes; + }; + + static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data); + + static const uint8_t serverSystemId = 50; ///< System ID for server + static const uint8_t serverComponentId = 1; ///< Component ID for server + static const uint8_t serverChannel = 0; ///< Channel to send to + + static const uint8_t clientSystemId = 1; ///< System ID for client + static const uint8_t clientComponentId = 0; ///< Component ID for client + + // We don't want any of these + MavlinkFtpTest(const MavlinkFtpTest &); + MavlinkFtpTest &operator=(const MavlinkFtpTest &); + +private: + virtual void _init(void); + virtual void _cleanup(void); + + bool _create_test_files(void); + bool _remove_test_files(void); + bool _ack_test(void); + bool _bad_opcode_test(void); + bool _bad_datasize_test(void); + bool _list_test(void); + bool _list_eof_test(void); + bool _open_badfile_test(void); + bool _open_terminate_test(void); + bool _terminate_badsession_test(void); + bool _read_test(void); + bool _read_badsession_test(void); + bool _burst_test(void); + bool _removedirectory_test(void); + bool _createdirectory_test(void); + bool _removefile_test(void); + + void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req); + void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, + mavlink_message_t *msg); + bool _decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, const MavlinkFTP::PayloadHeader **payload); + bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, + uint8_t size, + const uint8_t *data, + const MavlinkFTP::PayloadHeader **payload_reply); + void _cleanup_microsd(void); + + /// A single download test case + struct DownloadTestCase { + const char *file; + const uint16_t length; + bool singlePacketRead; + bool exactlyFillPacket; + }; + + /// The set of test cases for download testing + static const DownloadTestCase _rgDownloadTestCases[]; + + /// States for stream download handler + enum { + burst_state_first_ack, + burst_state_last_ack, + burst_state_nak_eof, + burst_state_complete + }; + + bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, BurstInfo *burst_info); + + MavlinkFTP *_ftp_server; + uint16_t _expected_seq_number; + + mavlink_file_transfer_protocol_t _reply_msg; + + static const char _unittest_microsd_dir[]; + static const char _unittest_microsd_file[]; +}; + +bool mavlink_ftp_test(void); diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp new file mode 100644 index 0000000..10cbcb0 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 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 mavlink_ftp_tests.cpp + */ + +#include + +#include "mavlink_ftp_test.h" + +extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); + +int mavlink_tests_main(int argc, char *argv[]) +{ + return mavlink_ftp_test() ? 0 : -1; +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_timesync.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_timesync.cpp new file mode 100644 index 0000000..6cd963a --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_timesync.cpp @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * 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 mavlink_timesync.cpp + * Mavlink timesync implementation. + * + * @author Mohammed Kabir + */ + +#include "mavlink_timesync.h" +#include "mavlink_main.h" + +#include + +MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) : + _mavlink(mavlink) +{ +} + +void +MavlinkTimesync::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_TIMESYNC: { + + mavlink_timesync_t tsync = {}; + mavlink_msg_timesync_decode(msg, &tsync); + + const uint64_t now = hrt_absolute_time(); + + if (tsync.tc1 == 0) { // Message originating from remote system, timestamp and return it + + mavlink_timesync_t rsync; + + rsync.tc1 = now * 1000ULL; + rsync.ts1 = tsync.ts1; + + mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync); + + return; + + } else if (tsync.tc1 > 0) { // Message originating from this system, compute time offset from it + + // Calculate time offset between this system and the remote system, assuming RTT for + // the timesync packet is roughly equal both ways. + int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ; + + // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system + uint64_t rtt_us = now - (tsync.ts1 / 1000ULL); + + // Calculate the difference of this sample from the current estimate + uint64_t deviation = llabs((int64_t)_time_offset - offset_us); + + if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT + + if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) { + + // Increment the counter if we have a good estimate and are getting samples far from the estimate + _high_deviation_count++; + + // We reset the filter if we received 5 consecutive samples which violate our present estimate. + // This is most likely due to a time jump on the offboard system. + if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) { + PX4_ERR("[timesync] Time jump detected. Resetting time synchroniser."); + // Reset the filter + reset_filter(); + } + + } else { + + // Filter gain scheduling + if (!sync_converged()) { + // Interpolate with a sigmoid function + double progress = (double)_sequence / (double)CONVERGENCE_WINDOW; + double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress))); + _filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL; + _filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL; + + } else { + _filter_alpha = ALPHA_GAIN_FINAL; + _filter_beta = BETA_GAIN_FINAL; + } + + // Perform filter update + add_sample(offset_us); + + // Increment sequence counter after filter update + _sequence++; + + // Reset high deviation count after filter update + _high_deviation_count = 0; + + // Reset high RTT count after filter update + _high_rtt_count = 0; + } + + } else { + // Increment counter if round trip time is too high for accurate timesync + _high_rtt_count++; + + if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) { + PX4_WARN("[timesync] RTT too high for timesync: %llu ms (sender: %i)", rtt_us / 1000ULL, msg->compid); + // Reset counter to rate-limit warnings + _high_rtt_count = 0; + } + + } + + // Publish status message + timesync_status_s tsync_status{}; + + tsync_status.timestamp = hrt_absolute_time(); + tsync_status.remote_timestamp = tsync.tc1 / 1000ULL; + tsync_status.observed_offset = offset_us; + tsync_status.estimated_offset = (int64_t)_time_offset; + tsync_status.round_trip_time = rtt_us; + + _timesync_status_pub.publish(tsync_status); + } + + break; + } + + case MAVLINK_MSG_ID_SYSTEM_TIME: { + + mavlink_system_time_t time; + mavlink_msg_system_time_decode(msg, &time); + + timespec tv = {}; + px4_clock_gettime(CLOCK_REALTIME, &tv); + + // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 + bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS; + bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; + + if (!onb_unix_valid && ofb_unix_valid) { + tv.tv_sec = time.time_unix_usec / 1000000ULL; + tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; + + if (px4_clock_settime(CLOCK_REALTIME, &tv)) { + PX4_ERR("[timesync] Failed setting realtime clock"); + } + } + + break; + } + + default: + break; + } +} + +uint64_t +MavlinkTimesync::sync_stamp(uint64_t usec) +{ + // Only return synchronised stamp if we have converged to a good value + if (sync_converged()) { + return usec + (int64_t)_time_offset; + + } else { + return hrt_absolute_time(); + } +} + +bool +MavlinkTimesync::sync_converged() +{ + return _sequence >= CONVERGENCE_WINDOW; +} + +void +MavlinkTimesync::add_sample(int64_t offset_us) +{ + /* Online exponential smoothing filter. The derivative of the estimate is also + * estimated in order to produce an estimate without steady state lag: + * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing + */ + + double time_offset_prev = _time_offset; + + if (_sequence == 0) { // First offset sample + _time_offset = offset_us; + + } else { + // Update the clock offset estimate + _time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew); + + // Update the clock skew estimate + _time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew; + } + +} + +void +MavlinkTimesync::reset_filter() +{ + // Do a full reset of all statistics and parameters + _sequence = 0; + _time_offset = 0.0; + _time_skew = 0.0; + _filter_alpha = ALPHA_GAIN_INITIAL; + _filter_beta = BETA_GAIN_INITIAL; + _high_deviation_count = 0; + _high_rtt_count = 0; + +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_timesync.h b/src/prometheus_px4/src/modules/mavlink/mavlink_timesync.h new file mode 100644 index 0000000..3204696 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_timesync.h @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * 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 mavlink_timesync.h + * Mavlink time synchroniser definition. + * + * @author Mohammed Kabir + */ + +#pragma once + +#include "mavlink_bridge_header.h" + +#include +#include + +#include + +#include +#include + +using namespace time_literals; + +static constexpr time_t PX4_EPOCH_SECS = 1234567890ULL; + +// Filter gains +// +// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead +// to a smoother estimate, but track time drift more slowly, introducing a bias +// in the estimate. Larger values will cause low-amplitude oscillations. +// +// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a +// tighter estimation of the skew (derivative), but will negatively affect how fast the +// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). +// Larger values will cause large-amplitude oscillations. +static constexpr double ALPHA_GAIN_INITIAL = 0.05; +static constexpr double BETA_GAIN_INITIAL = 0.05; +static constexpr double ALPHA_GAIN_FINAL = 0.003; +static constexpr double BETA_GAIN_FINAL = 0.003; + +// Filter gain scheduling +// +// The filter interpolates between the INITIAL and FINAL gains while the number of +// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will +// allow the timesync to converge faster, but with potentially less accurate initial +// offset and skew estimates. +static constexpr uint32_t CONVERGENCE_WINDOW = 500; + +// Outlier rejection and filter reset +// +// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter. +// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning +// but not reset the filter. +// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current +// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number +// of such events in a row will reset the filter. This usually happens only due to a time jump +// on the remote system. +// TODO : automatically determine these using ping statistics? +static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms; +static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms; +static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5; +static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5; + +class Mavlink; + +class MavlinkTimesync +{ +public: + explicit MavlinkTimesync(Mavlink *mavlink); + ~MavlinkTimesync() = default; + + void handle_message(const mavlink_message_t *msg); + + /** + * Convert remote timestamp to local hrt time (usec) + * Use synchronised time if available, monotonic boot time otherwise + */ + uint64_t sync_stamp(uint64_t usec); + +private: + + /* do not allow top copying this class */ + MavlinkTimesync(MavlinkTimesync &); + MavlinkTimesync &operator = (const MavlinkTimesync &); + +protected: + + /** + * Online exponential filter to smooth time offset + */ + void add_sample(int64_t offset_us); + + /** + * Return true if the timesync algorithm converged to a good estimate, + * return false otherwise + */ + bool sync_converged(); + + /** + * Reset the exponential filter and its states + */ + void reset_filter(); + + uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; + + uint32_t _sequence{0}; + + // Timesync statistics + double _time_offset{0}; + double _time_skew{0}; + + // Filter parameters + double _filter_alpha{ALPHA_GAIN_INITIAL}; + double _filter_beta{BETA_GAIN_INITIAL}; + + // Outlier rejection and filter reset + uint32_t _high_deviation_count{0}; + uint32_t _high_rtt_count{0}; + + Mavlink *const _mavlink; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_ulog.cpp b/src/prometheus_px4/src/modules/mavlink/mavlink_ulog.cpp new file mode 100644 index 0000000..1c4a10c --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_ulog.cpp @@ -0,0 +1,254 @@ +/**************************************************************************** + * + * Copyright (c) 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 mavlink_ulog.cpp + * ULog data streaming via MAVLink + * + * @author Beat Küng + */ + +#include "mavlink_ulog.h" +#include +#include +#include + +bool MavlinkULog::_init = false; +MavlinkULog *MavlinkULog::_instance = nullptr; +px4_sem_t MavlinkULog::_lock; +const float MavlinkULog::_rate_calculation_delta_t = 0.1f; + + +MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component) + : _target_system(target_system), _target_component(target_component), + _max_rate_factor(max_rate_factor), + _max_num_messages(math::max(1, (int)ceilf(_rate_calculation_delta_t *_max_rate_factor * datarate / + (MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))), + _current_rate_factor(max_rate_factor) +{ + // make sure we won't read any old messages + while (_ulog_stream_sub.update()) { + + } + + _waiting_for_initial_ack = true; + _last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization + _next_rate_check = _last_sent_time + _rate_calculation_delta_t * 1.e6f; +} + +void MavlinkULog::start_ack_received() +{ + if (_waiting_for_initial_ack) { + _last_sent_time = 0; + _waiting_for_initial_ack = false; + PX4_DEBUG("got logger ack"); + } +} + +int MavlinkULog::handle_update(mavlink_channel_t channel) +{ + static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN, + "Invalid uorb ulog_stream.data length"); + static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN, + "Invalid uorb ulog_stream.data length"); + + if (_waiting_for_initial_ack) { + if (hrt_elapsed_time(&_last_sent_time) > 3e5) { + PX4_WARN("no ack from logger (is it running?)"); + return -1; + } + + return 0; + } + + // check if we're waiting for an ACK + if (_last_sent_time) { + bool check_for_updates = false; + + if (_ack_received) { + _last_sent_time = 0; + check_for_updates = true; + + } else { + + if (hrt_elapsed_time(&_last_sent_time) > ulog_stream_ack_s::ACK_TIMEOUT * 1000) { + if (++_sent_tries > ulog_stream_ack_s::ACK_MAX_TRIES) { + return -ETIMEDOUT; + + } else { + PX4_DEBUG("re-sending ulog mavlink message (try=%i)", _sent_tries); + _last_sent_time = hrt_absolute_time(); + + const ulog_stream_s &ulog_data = _ulog_stream_sub.get(); + + mavlink_logging_data_acked_t msg; + msg.sequence = ulog_data.msg_sequence; + msg.length = ulog_data.length; + msg.first_message_offset = ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; + memcpy(msg.data, ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_acked_send_struct(channel, &msg); + } + } + } + + if (!check_for_updates) { + return 0; + } + } + + while ((_current_num_msgs < _max_num_messages) && _ulog_stream_sub.update()) { + const ulog_stream_s &ulog_data = _ulog_stream_sub.get(); + + if (ulog_data.timestamp > 0) { + if (ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) { + _sent_tries = 1; + _last_sent_time = hrt_absolute_time(); + lock(); + _wait_for_ack_sequence = ulog_data.msg_sequence; + _ack_received = false; + unlock(); + + mavlink_logging_data_acked_t msg; + msg.sequence = ulog_data.msg_sequence; + msg.length = ulog_data.length; + msg.first_message_offset = ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; + memcpy(msg.data, ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_acked_send_struct(channel, &msg); + + } else { + mavlink_logging_data_t msg; + msg.sequence = ulog_data.msg_sequence; + msg.length = ulog_data.length; + msg.first_message_offset = ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; + memcpy(msg.data, ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_send_struct(channel, &msg); + } + } + + ++_current_num_msgs; + } + + //need to update the rate? + hrt_abstime t = hrt_absolute_time(); + + if (t > _next_rate_check) { + if (_current_num_msgs < _max_num_messages) { + _current_rate_factor = _max_rate_factor * (float)_current_num_msgs / _max_num_messages; + + } else { + _current_rate_factor = _max_rate_factor; + } + + _current_num_msgs = 0; + _next_rate_check = t + _rate_calculation_delta_t * 1.e6f; + PX4_DEBUG("current rate=%.3f (max=%i msgs in %.3fs)", (double)_current_rate_factor, _max_num_messages, + (double)_rate_calculation_delta_t); + } + + return 0; +} + +void MavlinkULog::initialize() +{ + if (_init) { + return; + } + + px4_sem_init(&_lock, 1, 1); + _init = true; +} + +MavlinkULog *MavlinkULog::try_start(int datarate, float max_rate_factor, uint8_t target_system, + uint8_t target_component) +{ + MavlinkULog *ret = nullptr; + bool failed = false; + lock(); + + if (!_instance) { + ret = _instance = new MavlinkULog(datarate, max_rate_factor, target_system, target_component); + + if (!_instance) { + failed = true; + } + } + + unlock(); + + if (failed) { + PX4_ERR("alloc failed"); + } + + return ret; +} + +void MavlinkULog::stop() +{ + lock(); + + if (_instance) { + delete _instance; + _instance = nullptr; + } + + unlock(); +} + +void MavlinkULog::handle_ack(mavlink_logging_ack_t ack) +{ + lock(); + + if (_instance) { // make sure stop() was not called right before + if (_wait_for_ack_sequence == ack.sequence) { + _ack_received = true; + publish_ack(ack.sequence); + } + } + + unlock(); +} + +void MavlinkULog::publish_ack(uint16_t sequence) +{ + ulog_stream_ack_s ack; + ack.timestamp = hrt_absolute_time(); + ack.msg_sequence = sequence; + + _ulog_stream_ack_pub.publish(ack); +} diff --git a/src/prometheus_px4/src/modules/mavlink/mavlink_ulog.h b/src/prometheus_px4/src/modules/mavlink/mavlink_ulog.h new file mode 100644 index 0000000..c0b81b1 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/mavlink_ulog.h @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 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 mavlink_ulog.h + * ULog data streaming via MAVLink + * + * @author Beat Küng + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "mavlink_bridge_header.h" + +/** + * @class MavlinkULog + * ULog streaming class. At most one instance (stream) can exist, assigned to a specific mavlink channel. + */ +class MavlinkULog +{ +public: + /** + * initialize: call this once on startup (this function is not thread-safe!) + */ + static void initialize(); + + /** + * try to start a new stream. This fails if a stream is already running. + * thread-safe + * @param datarate maximum link data rate in B/s + * @param max_rate_factor let ulog streaming use a maximum of max_rate_factor * datarate + * @param target_system ID for mavlink message + * @param target_component ID for mavlink message + * @return instance, or nullptr + */ + static MavlinkULog *try_start(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component); + + /** + * stop the stream. It also deletes the singleton object, so make sure cleanup + * all pointers to it. + * thread-safe + */ + void stop(); + + /** + * periodic update method: check for ulog stream messages and handle retransmission. + * @return 0 on success, <0 otherwise + */ + int handle_update(mavlink_channel_t channel); + + /** ack from mavlink for a data message */ + void handle_ack(mavlink_logging_ack_t ack); + + /** this is called when we got an vehicle_command_ack from the logger */ + void start_ack_received(); + + float current_data_rate() const { return _current_rate_factor; } + float maximum_data_rate() const { return _max_rate_factor; } + +private: + + MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component); + + ~MavlinkULog() = default; + + static void lock() + { + do {} while (px4_sem_wait(&_lock) != 0); + } + + static void unlock() + { + px4_sem_post(&_lock); + } + + void publish_ack(uint16_t sequence); + + static px4_sem_t _lock; + static bool _init; + static MavlinkULog *_instance; + static const float _rate_calculation_delta_t; ///< rate update interval + + uORB::SubscriptionData _ulog_stream_sub{ORB_ID(ulog_stream)}; + uORB::Publication _ulog_stream_ack_pub{ORB_ID(ulog_stream_ack)}; + uint16_t _wait_for_ack_sequence; + uint8_t _sent_tries = 0; + volatile bool _ack_received = false; ///< set to true if a matching ack received + hrt_abstime _last_sent_time = 0; ///< last time we sent a message that requires an ack + bool _waiting_for_initial_ack = false; + const uint8_t _target_system; + const uint8_t _target_component; + + const float _max_rate_factor; ///< maximum rate percentage at which we're allowed to push data + const int _max_num_messages; ///< maximum number of messages we can send within _rate_calculation_delta_t + float _current_rate_factor; ///< currently used rate percentage + int _current_num_msgs = 0; ///< number of messages sent within the current time interval + hrt_abstime _next_rate_check; ///< next timestamp at which to update the rate + + /* do not allow copying this class */ + MavlinkULog(const MavlinkULog &) = delete; + MavlinkULog operator=(const MavlinkULog &) = delete; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/module.yaml b/src/prometheus_px4/src/modules/mavlink/module.yaml new file mode 100644 index 0000000..53794aa --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/module.yaml @@ -0,0 +1,155 @@ +__max_num_config_instances: &max_num_config_instances 3 + +module_name: MAVLink +serial_config: + - command: | + if [ $SERIAL_DEV != ethernet ]; then + set MAV_ARGS "-d ${SERIAL_DEV} -b p:${BAUD_PARAM} -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE" + else + set MAV_ARGS "-u p:MAV_${i}_UDP_PRT -o p:MAV_${i}_REMOTE_PRT -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE" + if param compare MAV_${i}_BROADCAST 1 + then + set MAV_ARGS "${MAV_ARGS} -p" + fi + if param compare MAV_${i}_BROADCAST 2 + then + set MAV_ARGS "${MAV_ARGS} -c" + fi + fi + if param compare MAV_${i}_FORWARD 1 + then + set MAV_ARGS "${MAV_ARGS} -f" + fi + if param compare MAV_${i}_RADIO_CTL 1 + then + set MAV_ARGS "${MAV_ARGS} -s" + fi + mavlink start ${MAV_ARGS} -x + port_config_param: + name: MAV_${i}_CONFIG + group: MAVLink + # MAVLink instances: + # 0: Telem1 Port (Telemetry Link) + # 1: Telem2 Port (Companion Link). Disabled by default to reduce RAM usage + # 2: Board-specific / no fixed function or port + default: [TEL1, "", ""] + num_instances: *max_num_config_instances + supports_networking: true + +parameters: + - group: MAVLink + definitions: + MAV_${i}_MODE: + description: + short: MAVLink Mode for instance ${i} + long: | + The MAVLink Mode defines the set of streamed messages (for example the + vehicle's attitude) and their sending rates. + + type: enum + values: + 0: Normal + 1: Custom + 2: Onboard + 3: OSD + 4: Magic + 5: Config + #6: Iridium # as the user does not need to configure this, hide it from the UI + 7: Minimal + 8: External Vision + #9: External Vision Minimal # hidden + 10: Gimbal + 11: Onboard Low Bandwidth + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 2, 0] + + MAV_${i}_RATE: + description: + short: Maximum MAVLink sending rate for instance ${i} + long: | + Configure the maximum sending rate for the MAVLink streams in Bytes/sec. + If the configured streams exceed the maximum rate, the sending rate of + each stream is automatically decreased. + + If this is set to 0 a value of half of the theoretical maximum bandwidth is used. + This corresponds to baudrate/20 Bytes/s (baudrate/10 = maximum data rate on + 8N1-configured links). + + type: int32 + min: 0 + unit: B/s + reboot_required: true + num_instances: *max_num_config_instances + default: [1200, 0, 0] + + MAV_${i}_FORWARD: + description: + short: Enable MAVLink Message forwarding for instance ${i} + long: | + If enabled, forward incoming MAVLink messages to other MAVLink ports if the + message is either broadcast or the target is not the autopilot. + + This allows for example a GCS to talk to a camera that is connected to the + autopilot via MAVLink (on a different link than the GCS). + + type: boolean + reboot_required: true + num_instances: *max_num_config_instances + default: [true, false, false] + + MAV_${i}_RADIO_CTL: + description: + short: Enable software throttling of mavlink on instance ${i} + long: | + If enabled, MAVLink messages will be throttled according to + `txbuf` field reported by radio_status. + + Requires a radio to send the mavlink message RADIO_STATUS. + + type: boolean + reboot_required: true + num_instances: *max_num_config_instances + default: [true, true, true] + + MAV_${i}_UDP_PRT: + description: + short: MAVLink Network Port for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected udp port will be set and used in MAVLink instance ${i}. + + type: int32 + reboot_required: true + num_instances: *max_num_config_instances + default: [14556, 0, 0] + requires_ethernet: true + + MAV_${i}_REMOTE_PRT: + description: + short: MAVLink Remote Port for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected remote port will be set and used in MAVLink instance ${i}. + + type: int32 + reboot_required: true + num_instances: *max_num_config_instances + default: [14550, 0, 0] + requires_ethernet: true + + MAV_${i}_BROADCAST: + description: + short: Broadcast heartbeats on local network for MAVLink instance ${i} + long: | + This allows a ground control station to automatically find the drone + on the local network. + + type: enum + values: + 0: Never broadcast + 1: Always broadcast + 2: Only multicast + num_instances: *max_num_config_instances + default: [1, 0, 0] + requires_ethernet: true diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp new file mode 100644 index 0000000..64f6eb9 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ACTUATOR_CONTROL_TARGET_HPP +#define ACTUATOR_CONTROL_TARGET_HPP + +#include + +template +class MavlinkStreamActuatorControlTarget : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamActuatorControlTarget(mavlink); } + + static constexpr const char *get_name_static() + { + switch (N) { + case 0: + return "ACTUATOR_CONTROL_TARGET0"; + + case 1: + return "ACTUATOR_CONTROL_TARGET1"; + + case 2: + return "ACTUATOR_CONTROL_TARGET2"; + + case 3: + return "ACTUATOR_CONTROL_TARGET3"; + } + } + + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return (_act_ctrl_sub + && _act_ctrl_sub->advertised()) ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamActuatorControlTarget(Mavlink *mavlink) : MavlinkStream(mavlink) + { + // XXX this can be removed once the multiplatform system remaps topics + switch (N) { + case 0: + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_0)}; + break; + + case 1: + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_1)}; + break; + + case 2: + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_2)}; + break; + + case 3: + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_3)}; + break; + } + } + + ~MavlinkStreamActuatorControlTarget() override + { + delete _act_ctrl_sub; + } + + uORB::Subscription *_act_ctrl_sub{nullptr}; + + bool send() override + { + actuator_controls_s act_ctrl; + + if (_act_ctrl_sub && _act_ctrl_sub->update(&act_ctrl)) { + mavlink_actuator_control_target_t msg{}; + + msg.time_usec = act_ctrl.timestamp; + msg.group_mlx = N; + + for (unsigned i = 0; i < sizeof(msg.controls) / sizeof(msg.controls[0]); i++) { + msg.controls[i] = act_ctrl.control[i]; + } + + mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ACTUATOR_CONTROL_TARGET_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp new file mode 100644 index 0000000..5c5cfbd --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ACTUATOR_OUTPUT_STATUS_HPP +#define ACTUATOR_OUTPUT_STATUS_HPP + +#include + +class MavlinkStreamActuatorOutputStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamActuatorOutputStatus(mavlink); } + + static constexpr const char *get_name_static() { return "ACTUATOR_OUTPUT_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _act_output_sub.advertised() ? (MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _act_output_sub{ORB_ID(actuator_outputs)}; + + bool send() override + { + actuator_outputs_s act; + + if (_act_output_sub.update(&act)) { + mavlink_actuator_output_status_t msg{}; + + msg.time_usec = act.timestamp; + msg.active = act.noutputs; + + static size_t actuator_outputs_size = act.noutputs; + static constexpr size_t mavlink_actuator_output_status_size = sizeof(msg.actuator) / sizeof(msg.actuator[0]); + + for (unsigned i = 0; i < math::min(actuator_outputs_size, mavlink_actuator_output_status_size); i++) { + msg.actuator[i] = act.output[i]; + } + + mavlink_msg_actuator_output_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ACTUATOR_OUTPUT_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ADSB_VEHICLE.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ADSB_VEHICLE.hpp new file mode 100644 index 0000000..56f2a94 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ADSB_VEHICLE.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ADSB_VEHICLE_HPP +#define ADSB_VEHICLE_HPP + +#include + +class MavlinkStreamADSBVehicle : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamADSBVehicle(mavlink); } + + static constexpr const char *get_name_static() { return "ADSB_VEHICLE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ADSB_VEHICLE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + bool const_rate() override { return true; } + + unsigned get_size() override + { + return _transponder_report_sub.advertised() ? MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _transponder_report_sub{ORB_ID(transponder_report)}; + + bool send() override + { + bool sent = false; + + transponder_report_s pos; + + while ((_mavlink->get_free_tx_buf() >= get_size()) && _transponder_report_sub.update(&pos)) { + + if (!(pos.flags & transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE)) { + continue; + } + + mavlink_adsb_vehicle_t msg{}; + msg.ICAO_address = pos.icao_address; + msg.lat = pos.lat * 1e7; + msg.lon = pos.lon * 1e7; + msg.altitude_type = pos.altitude_type; + msg.altitude = pos.altitude * 1e3f; + msg.heading = (pos.heading + M_PI_F) / M_PI_F * 180.0f * 100.0f; + msg.hor_velocity = pos.hor_velocity * 100.0f; + msg.ver_velocity = pos.ver_velocity * 100.0f; + memcpy(&msg.callsign[0], &pos.callsign[0], sizeof(msg.callsign)); + msg.emitter_type = pos.emitter_type; + msg.tslc = pos.tslc; + msg.squawk = pos.squawk; + + msg.flags = 0; + + if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS) { msg.flags |= ADSB_FLAGS_VALID_COORDS; } + + if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE) { msg.flags |= ADSB_FLAGS_VALID_ALTITUDE; } + + if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING) { msg.flags |= ADSB_FLAGS_VALID_HEADING; } + + if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY) { msg.flags |= ADSB_FLAGS_VALID_VELOCITY; } + + if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) { msg.flags |= ADSB_FLAGS_VALID_CALLSIGN; } + + if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK) { msg.flags |= ADSB_FLAGS_VALID_SQUAWK; } + + mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg); + sent = true; + } + + return sent; + } +}; + +#endif // ADSB_VEHICLE_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ALTITUDE.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ALTITUDE.hpp new file mode 100644 index 0000000..f3c437d --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ALTITUDE.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ALTITUDE_HPP +#define ALTITUDE_HPP + +#include +#include +#include +#include + +class MavlinkStreamAltitude : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAltitude(mavlink); } + + static constexpr const char *get_name_static() { return "ALTITUDE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ALTITUDE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _local_pos_sub.advertised() ? MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _home_sub{ORB_ID(home_position)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + + bool send() override + { + mavlink_altitude_t msg{}; + + msg.altitude_monotonic = NAN; + msg.altitude_amsl = NAN; + msg.altitude_local = NAN; + msg.altitude_relative = NAN; + msg.altitude_terrain = NAN; + msg.bottom_clearance = NAN; + + // always update monotonic altitude + bool air_data_updated = false; + vehicle_air_data_s air_data{}; + _air_data_sub.copy(&air_data); + + if (air_data.timestamp > 0) { + msg.altitude_monotonic = air_data.baro_alt_meter; + + air_data_updated = true; + } + + bool lpos_updated = false; + + vehicle_local_position_s local_pos{}; + + if (_local_pos_sub.copy(&local_pos)) { + + if (local_pos.z_valid) { + if (local_pos.z_global) { + msg.altitude_amsl = -local_pos.z + local_pos.ref_alt; + + } else { + msg.altitude_amsl = msg.altitude_monotonic; + } + + msg.altitude_local = -local_pos.z; + + home_position_s home{}; + _home_sub.copy(&home); + + if (home.valid_alt) { + msg.altitude_relative = -(local_pos.z - home.z); + + } else { + msg.altitude_relative = -local_pos.z; + } + + if (local_pos.dist_bottom_valid) { + msg.altitude_terrain = -local_pos.z - local_pos.dist_bottom; + msg.bottom_clearance = local_pos.dist_bottom; + } + } + + lpos_updated = true; + } + + // local position timeout after 10 ms + // avoid publishing only baro altitude_monotonic if possible + bool lpos_timeout = (hrt_elapsed_time(&local_pos.timestamp) > 10_ms); + + if (lpos_updated || (air_data_updated && lpos_timeout)) { + msg.time_usec = hrt_absolute_time(); + mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ALTITUDE_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE.hpp new file mode 100644 index 0000000..94c1f4a --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ATTITUDE_HPP +#define ATTITUDE_HPP + +#include +#include + +class MavlinkStreamAttitude : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitude(mavlink); } + + static constexpr const char *get_name_static() { return "ATTITUDE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _att_sub.advertised() ? MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + + bool send() override + { + vehicle_attitude_s att; + + if (_att_sub.update(&att)) { + vehicle_angular_velocity_s angular_velocity{}; + _angular_velocity_sub.copy(&angular_velocity); + + mavlink_attitude_t msg{}; + + const matrix::Eulerf euler = matrix::Quatf(att.q); + msg.time_boot_ms = att.timestamp / 1000; + msg.roll = euler.phi(); + msg.pitch = euler.theta(); + msg.yaw = euler.psi(); + + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; + + mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ATTITUDE_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp new file mode 100644 index 0000000..6418941 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ATTITUDE_QUATERNION_HPP +#define ATTITUDE_QUATERNION_HPP + +#include +#include +#include + +class MavlinkStreamAttitudeQuaternion : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeQuaternion(mavlink); } + + static constexpr const char *get_name_static() { return "ATTITUDE_QUATERNION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _att_sub.advertised() ? MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + + bool send() override + { + vehicle_attitude_s att; + + if (_att_sub.update(&att)) { + vehicle_angular_velocity_s angular_velocity{}; + _angular_velocity_sub.copy(&angular_velocity); + + vehicle_status_s status{}; + _status_sub.copy(&status); + + mavlink_attitude_quaternion_t msg{}; + + msg.time_boot_ms = att.timestamp / 1000; + msg.q1 = att.q[0]; + msg.q2 = att.q[1]; + msg.q3 = att.q[2]; + msg.q4 = att.q[3]; + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; + + if (status.is_vtol && status.is_vtol_tailsitter && (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + // This is a tailsitter VTOL flying in fixed wing mode: + // indicate that reported attitude should be rotated by + // 90 degrees upward pitch for user display + get_rot_quaternion(ROTATION_PITCH_90).copyTo(msg.repr_offset_q); + + } else { + // Normal case + // zero rotation should be [1 0 0 0]: + // `get_rot_quaternion(ROTATION_NONE).copyTo(msg.repr_offset_q);` + // but to save bandwidth, we instead send [0, 0, 0, 0]. + msg.repr_offset_q[0] = 0.0f; + msg.repr_offset_q[1] = 0.0f; + msg.repr_offset_q[2] = 0.0f; + msg.repr_offset_q[3] = 0.0f; + } + + mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ATTITUDE_QUATERNION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp new file mode 100644 index 0000000..dd93fc8 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ATTITUDE_TARGET_HPP +#define ATTITUDE_TARGET_HPP + +#include +#include + +class MavlinkStreamAttitudeTarget : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeTarget(mavlink); } + + static constexpr const char *get_name_static() { return "ATTITUDE_TARGET"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATTITUDE_TARGET; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _att_sp_sub.advertised() ? MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _att_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; + hrt_abstime _last_att_sp_update{0}; + + bool send() override + { + vehicle_attitude_setpoint_s att_sp; + + bool updated = false; + + if (_att_sp_sub.update(&att_sp)) { + _last_att_sp_update = att_sp.timestamp; + updated = true; + + } else if (hrt_elapsed_time(&_last_att_sp_update) > 500_ms) { + if (!_att_sp_sub.copy(&att_sp)) { + att_sp = {}; + } + + updated = _att_rates_sp_sub.updated(); + } + + if (updated) { + mavlink_attitude_target_t msg{}; + + msg.time_boot_ms = att_sp.timestamp / 1000; + matrix::Quatf(att_sp.q_d).copyTo(msg.q); + + vehicle_rates_setpoint_s att_rates_sp{}; + _att_rates_sp_sub.copy(&att_rates_sp); + + msg.body_roll_rate = att_rates_sp.roll; + msg.body_pitch_rate = att_rates_sp.pitch; + msg.body_yaw_rate = att_rates_sp.yaw; + + msg.thrust = matrix::Vector3f(att_sp.thrust_body).norm(); + + mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ATTITUDE_TARGET_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ATT_POS_MOCAP.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ATT_POS_MOCAP.hpp new file mode 100644 index 0000000..07ea7a9 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ATT_POS_MOCAP.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ATT_POS_MOCAP_HPP +#define ATT_POS_MOCAP_HPP + +#include + +class MavlinkStreamAttPosMocap : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttPosMocap(mavlink); } + + static constexpr const char *get_name_static() { return "ATT_POS_MOCAP"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATT_POS_MOCAP; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _mocap_sub.advertised() ? MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _mocap_sub{ORB_ID(vehicle_mocap_odometry)}; + + bool send() override + { + vehicle_odometry_s mocap; + + if (_mocap_sub.update(&mocap)) { + mavlink_att_pos_mocap_t msg{}; + + msg.time_usec = mocap.timestamp_sample; + msg.q[0] = mocap.q[0]; + msg.q[1] = mocap.q[1]; + msg.q[2] = mocap.q[2]; + msg.q[3] = mocap.q[3]; + msg.x = mocap.x; + msg.y = mocap.y; + msg.z = mocap.z; + // msg.covariance = + + mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ATT_POS_MOCAP_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp b/src/prometheus_px4/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp new file mode 100644 index 0000000..5e4dea2 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP +#define AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP + +#include +#include +#include +#include +#include +#include + +class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAutopilotStateForGimbalDevice(mavlink); } + + static constexpr const char *get_name_static() { return "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_att_sub.advertised()) { + return MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; + + bool send() override + { + vehicle_attitude_s att; + + if (_att_sub.update(&att)) { + mavlink_autopilot_state_for_gimbal_device_t msg{}; + + //msg.target_system = 0; // TODO + //msg.target_component = 0; // TODO + + msg.time_boot_us = att.timestamp; + msg.q[0] = att.q[0]; + msg.q[1] = att.q[1]; + msg.q[2] = att.q[2]; + msg.q[3] = att.q[3]; + msg.q_estimated_delay_us = 0; // I don't know. + + { + vehicle_local_position_s lpos; + + if (_lpos_sub.copy(&lpos)) { + msg.vx = lpos.vx; + msg.vy = lpos.vy; + msg.vz = lpos.vz; + msg.v_estimated_delay_us = 0; // I don't know. + } + } + + { + vehicle_attitude_setpoint_s att_sp; + + if (_att_sp_sub.copy(&att_sp)) { + msg.feed_forward_angular_velocity_z = att_sp.yaw_sp_move_rate; + } + } + + { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.update(&estimator_selector_status)) { + _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance); + + } + + estimator_status_s est; + + if (_estimator_status_sub.copy(&est)) { + msg.estimator_status = est.solution_status_flags; + } + } + + { + vehicle_land_detected_s land_detected; + + if (_landed_sub.copy(&land_detected)) { + // Ignore take-off and landing states for now. + msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR; + } + } + + mavlink_msg_autopilot_state_for_gimbal_device_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/AUTOPILOT_VERSION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/AUTOPILOT_VERSION.hpp new file mode 100644 index 0000000..efcc730 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/AUTOPILOT_VERSION.hpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef AUTOPILOT_VERSION_HPP +#define AUTOPILOT_VERSION_HPP + +class MavlinkStreamAutopilotVersion : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAutopilotVersion(mavlink); } + + static constexpr const char *get_name_static() { return "AUTOPILOT_VERSION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_AUTOPILOT_VERSION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamAutopilotVersion(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + bool send() override + { + return _mavlink->send_autopilot_capabilities(); + } +}; + +#endif // AUTOPILOT_VERSION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/BATTERY_STATUS.hpp new file mode 100644 index 0000000..c0fefed --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef BATTERY_STATUS_HPP +#define BATTERY_STATUS_HPP + +#include + +class MavlinkStreamBatteryStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamBatteryStatus(mavlink); } + + static constexpr const char *get_name_static() { return "BATTERY_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_BATTERY_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_BATTERY_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return size_per_battery * _battery_status_subs.advertised_count(); + } + +private: + explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + + bool send() override + { + bool updated = false; + + for (auto &battery_sub : _battery_status_subs) { + battery_status_s battery_status; + + if (battery_sub.update(&battery_status)) { + /* battery status message with higher resolution */ + mavlink_battery_status_t bat_msg{}; + // TODO: Determine how to better map between battery ID within the firmware and in MAVLink + bat_msg.id = battery_status.id - 1; + bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL; + bat_msg.type = MAV_BATTERY_TYPE_LIPO; + bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1; + bat_msg.energy_consumed = -1; + bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1; + bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.f) : -1; + bat_msg.time_remaining = (battery_status.connected) ? battery_status.run_time_to_empty * 60 : 0; + + switch (battery_status.warning) { + case (battery_status_s::BATTERY_WARNING_NONE): + bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_OK; + break; + + case (battery_status_s::BATTERY_WARNING_LOW): + bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_LOW; + break; + + case (battery_status_s::BATTERY_WARNING_CRITICAL): + bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CRITICAL; + break; + + case (battery_status_s::BATTERY_WARNING_EMERGENCY): + bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_EMERGENCY; + break; + + case (battery_status_s::BATTERY_WARNING_FAILED): + bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_FAILED; + break; + + default: + bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_UNDEFINED; + break; + } + + // check if temperature valid + if (battery_status.connected && PX4_ISFINITE(battery_status.temperature)) { + bat_msg.temperature = battery_status.temperature * 100.f; + + } else { + bat_msg.temperature = INT16_MAX; + } + + static constexpr int mavlink_cells_max = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); + static constexpr int uorb_cells_max = + (sizeof(battery_status.voltage_cell_v) / sizeof(battery_status.voltage_cell_v[0])); + + for (int cell = 0; cell < mavlink_cells_max; cell++) { + if (battery_status.connected && (cell < battery_status.cell_count) && (cell < uorb_cells_max)) { + bat_msg.voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f; + + } else { + bat_msg.voltages[cell] = UINT16_MAX; + } + } + + static constexpr int mavlink_cells_ext_max = (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0])); + + for (int cell = mavlink_cells_max; cell < mavlink_cells_max + mavlink_cells_ext_max; cell++) { + if (battery_status.connected && (cell < battery_status.cell_count) && (cell < uorb_cells_max)) { + bat_msg.voltages_ext[cell - mavlink_cells_max] = battery_status.voltage_cell_v[cell] * 1000.0f; + + } else { + bat_msg.voltages_ext[cell - mavlink_cells_max] = UINT16_MAX; + } + } + + mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg); + updated = true; + } + } + + return updated; + } +}; + +#endif // BATTERY_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp b/src/prometheus_px4/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp new file mode 100644 index 0000000..fede0f3 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef CAMERA_IMAGE_CAPTURED_HPP +#define CAMERA_IMAGE_CAPTURED_HPP + +#include + +class MavlinkStreamCameraImageCaptured : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCameraImageCaptured(mavlink); } + + static constexpr const char *get_name_static() { return "CAMERA_IMAGE_CAPTURED"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + bool const_rate() override { return true; } + + unsigned get_size() override + { + return _capture_sub.advertised() ? MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamCameraImageCaptured(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _capture_sub{ORB_ID(camera_capture)}; + + bool send() override + { + camera_capture_s capture; + + if ((_mavlink->get_free_tx_buf() >= get_size()) && _capture_sub.update(&capture)) { + mavlink_camera_image_captured_t msg{}; + + msg.time_boot_ms = capture.timestamp / 1000; + msg.time_utc = capture.timestamp_utc; + msg.camera_id = 1; // FIXME : get this from uORB + msg.lat = capture.lat * 1e7; + msg.lon = capture.lon * 1e7; + msg.alt = capture.alt * 1e3f; + msg.relative_alt = capture.ground_distance * 1e3f; + msg.q[0] = capture.q[0]; + msg.q[1] = capture.q[1]; + msg.q[2] = capture.q[2]; + msg.q[3] = capture.q[3]; + msg.image_index = capture.seq; + msg.capture_result = capture.result; + msg.file_url[0] = '\0'; + + mavlink_msg_camera_image_captured_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // CAMERA_IMAGE_CAPTURED_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp b/src/prometheus_px4/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp new file mode 100644 index 0000000..64d199e --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef CAMERA_TRIGGER_HPP +#define CAMERA_TRIGGER_HPP + +#include + +class MavlinkStreamCameraTrigger : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCameraTrigger(mavlink); } + + static constexpr const char *get_name_static() { return "CAMERA_TRIGGER"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CAMERA_TRIGGER; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + bool const_rate() override { return true; } + + unsigned get_size() override + { + if (_camera_trigger_sub.advertised()) { + return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + + MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; // TODO: MAV_CMD_DO_DIGICAM_CONTROL + } + + return 0; + } + +private: + explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _camera_trigger_sub{ORB_ID(camera_trigger)}; + + bool send() override + { + camera_trigger_s camera_trigger; + + if ((_mavlink->get_free_tx_buf() >= get_size()) && _camera_trigger_sub.update(&camera_trigger)) { + /* ensure that only active trigger events are sent */ + if (camera_trigger.timestamp > 0) { + mavlink_camera_trigger_t msg{}; + msg.time_usec = camera_trigger.timestamp; + msg.seq = camera_trigger.seq; + mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg); + + + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); + vcmd.param1 = 0.0f; // all cameras + vcmd.param2 = 0.0f; // duration 0 because only taking one picture + vcmd.param3 = 1.0f; // only take one + vcmd.param4 = NAN; + vcmd.param5 = (double)NAN; + vcmd.param6 = (double)NAN; + vcmd.param7 = NAN; + vcmd.command = MAV_CMD_IMAGE_START_CAPTURE; + vcmd.target_system = mavlink_system.sysid; + vcmd.target_component = MAV_COMP_ID_CAMERA; + + MavlinkCommandSender::instance().handle_vehicle_command(vcmd, _mavlink->get_channel()); + + + // TODO: move this camera_trigger and publish as a vehicle_command + /* send MAV_CMD_DO_DIGICAM_CONTROL*/ + mavlink_command_long_t command_long_msg{}; + + command_long_msg.target_system = 0; // 0 for broadcast + command_long_msg.target_component = MAV_COMP_ID_CAMERA; + command_long_msg.command = MAV_CMD_DO_DIGICAM_CONTROL; + command_long_msg.confirmation = 0; + command_long_msg.param1 = NAN; + command_long_msg.param2 = NAN; + command_long_msg.param3 = NAN; + command_long_msg.param4 = NAN; + command_long_msg.param5 = 1; // take 1 picture + command_long_msg.param6 = NAN; + command_long_msg.param7 = NAN; + + mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &command_long_msg); + + return true; + } + } + + return false; + } +}; + +#endif // CAMERA_TRIGGER_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/COLLISION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/COLLISION.hpp new file mode 100644 index 0000000..a3469c6 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/COLLISION.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef COLLISION_HPP +#define COLLISION_HPP + +#include + +class MavlinkStreamCollision : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCollision(mavlink); } + + static constexpr const char *get_name_static() { return "COLLISION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_COLLISION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _collision_sub.advertised() ? MAVLINK_MSG_ID_COLLISION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _collision_sub{ORB_ID(collision_report)}; + + bool send() override + { + collision_report_s report; + bool sent = false; + + while ((_mavlink->get_free_tx_buf() >= get_size()) && _collision_sub.update(&report)) { + mavlink_collision_t msg = {}; + + msg.src = report.src; + msg.id = report.id; + msg.action = report.action; + msg.threat_level = report.threat_level; + msg.time_to_minimum_delta = report.time_to_minimum_delta; + msg.altitude_minimum_delta = report.altitude_minimum_delta; + msg.horizontal_minimum_delta = report.horizontal_minimum_delta; + + mavlink_msg_collision_send_struct(_mavlink->get_channel(), &msg); + sent = true; + } + + return sent; + } +}; + +#endif // COLLISION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/COMMAND_LONG.hpp b/src/prometheus_px4/src/modules/mavlink/streams/COMMAND_LONG.hpp new file mode 100644 index 0000000..bd36968 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/COMMAND_LONG.hpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef COMMAND_LONG_HPP +#define COMMAND_LONG_HPP + +#include + +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCommandLong(mavlink); } + + static constexpr const char *get_name_static() { return "COMMAND_LONG"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_COMMAND_LONG; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return 0; // commands stream is not regular and not predictable + } + +private: + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + + bool send() override + { + bool sent = false; + + static constexpr size_t COMMAND_LONG_SIZE = MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + while ((_mavlink->get_free_tx_buf() >= COMMAND_LONG_SIZE) && _vehicle_command_sub.updated()) { + + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); + vehicle_command_s cmd; + + if (_vehicle_command_sub.update(&cmd)) { + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation, + _vehicle_command_sub.get_last_generation()); + } + + if (!cmd.from_external && cmd.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { + PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + + MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); + sent = true; + + } else { + PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); + } + } + } + + MavlinkCommandSender::instance().check_timeout(_mavlink->get_channel()); + + return sent; + } +}; + +#endif // COMMAND_LONG_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/COMPONENT_INFORMATION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/COMPONENT_INFORMATION.hpp new file mode 100644 index 0000000..d4bc184 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/COMPONENT_INFORMATION.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef COMPONENT_INFORMATION_HPP +#define COMPONENT_INFORMATION_HPP + +#include "../mavlink_stream.h" + +#include + +#include + +#include + +class MavlinkStreamComponentInformation : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamComponentInformation(mavlink); } + + static constexpr const char *get_name_static() { return "COMPONENT_INFORMATION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_COMPONENT_INFORMATION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return 0; // never streamed + } + + bool request_message(float param2, float param3, float param4, + float param5, float param6, float param7) override + { + mavlink_component_information_t component_info{}; + PX4_DEBUG("COMPONENT_INFORMATION request"); + + strncpy(component_info.general_metadata_uri, "mftp://etc/extras/component_general.json.xz", + sizeof(component_info.general_metadata_uri) - 1); + component_info.general_metadata_file_crc = component_information::component_general_crc; + + component_info.time_boot_ms = hrt_absolute_time() / 1000; + mavlink_msg_component_information_send_struct(_mavlink->get_channel(), &component_info); + + return true; + } +private: + explicit MavlinkStreamComponentInformation(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + bool send() override + { + return false; + } +}; + +#endif // COMPONENT_INFORMATION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/DEBUG.hpp b/src/prometheus_px4/src/modules/mavlink/streams/DEBUG.hpp new file mode 100644 index 0000000..c0c2a52 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/DEBUG.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef DEBUG_HPP +#define DEBUG_HPP + +#include + +class MavlinkStreamDebug : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDebug(mavlink); } + + static constexpr const char *get_name_static() { return "DEBUG"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DEBUG; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _debug_value_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _debug_value_sub{ORB_ID(debug_value)}; + + bool send() override + { + debug_value_s debug; + + if (_debug_value_sub.update(&debug)) { + mavlink_debug_t msg{}; + msg.time_boot_ms = debug.timestamp / 1000ULL; + msg.ind = debug.ind; + msg.value = debug.value; + + mavlink_msg_debug_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // DEBUG_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp b/src/prometheus_px4/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp new file mode 100644 index 0000000..a5029ff --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef DEBUG_FLOAT_ARRAY_HPP +#define DEBUG_FLOAT_ARRAY_HPP + +#include + +class MavlinkStreamDebugFloatArray : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDebugFloatArray(mavlink); } + + static constexpr const char *get_name_static() { return "DEBUG_FLOAT_ARRAY"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _debug_array_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _debug_array_sub{ORB_ID(debug_array)}; + + bool send() override + { + debug_array_s debug; + + if (_debug_array_sub.update(&debug)) { + mavlink_debug_float_array_t msg{}; + + msg.time_usec = debug.timestamp; + msg.array_id = debug.id; + memcpy(msg.name, debug.name, sizeof(msg.name)); + msg.name[sizeof(msg.name) - 1] = '\0'; // enforce null termination + + for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) { + msg.data[i] = debug.data[i]; + } + + mavlink_msg_debug_float_array_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // DEBUG_FLOAT_ARRAY_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/DEBUG_VECT.hpp b/src/prometheus_px4/src/modules/mavlink/streams/DEBUG_VECT.hpp new file mode 100644 index 0000000..9fa2931 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/DEBUG_VECT.hpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef DEBUG_VECT_HPP +#define DEBUG_VECT_HPP + +#include + +class MavlinkStreamDebugVect : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDebugVect(mavlink); } + + static constexpr const char *get_name_static() { return "DEBUG_VECT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DEBUG_VECT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _debug_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_VECT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _debug_sub{ORB_ID(debug_vect)}; + + bool send() override + { + debug_vect_s debug; + + if (_debug_sub.update(&debug)) { + mavlink_debug_vect_t msg{}; + msg.time_usec = debug.timestamp; + memcpy(msg.name, debug.name, sizeof(msg.name)); + msg.name[sizeof(msg.name) - 1] = '\0'; // enforce null termination + msg.x = debug.x; + msg.y = debug.y; + msg.z = debug.z; + + mavlink_msg_debug_vect_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // DEBUG_VECT_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp b/src/prometheus_px4/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp new file mode 100644 index 0000000..9042403 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef DISTANCE_SENSOR_HPP +#define DISTANCE_SENSOR_HPP + +#include +#include + +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDistanceSensor(mavlink); } + + static constexpr const char *get_name_static() { return "DISTANCE_SENSOR"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DISTANCE_SENSOR; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _distance_sensor_subs.advertised_count() * (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + + bool send() override + { + bool updated = false; + + for (int i = 0; i < _distance_sensor_subs.size(); i++) { + distance_sensor_s dist_sensor; + + if (_distance_sensor_subs[i].update(&dist_sensor)) { + mavlink_distance_sensor_t msg{}; + + msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */ + + switch (dist_sensor.type) { + case MAV_DISTANCE_SENSOR_ULTRASOUND: + msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND; + break; + + case MAV_DISTANCE_SENSOR_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + case MAV_DISTANCE_SENSOR_INFRARED: + msg.type = MAV_DISTANCE_SENSOR_INFRARED; + break; + + default: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + msg.current_distance = dist_sensor.current_distance * 1e2f; // m to cm + msg.id = i; + msg.max_distance = dist_sensor.max_distance * 1e2f; // m to cm + msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm + msg.orientation = dist_sensor.orientation; + msg.covariance = dist_sensor.variance * 1e4f; // m^2 to cm^2 + + mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg); + + updated = true; + } + } + + return updated; + } +}; + +#endif // DISTANCE_SENSOR diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ESC_INFO.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ESC_INFO.hpp new file mode 100644 index 0000000..fc57f29 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ESC_INFO.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ESC_INFO_HPP +#define ESC_INFO_HPP + +#include + +class MavlinkStreamESCInfo : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamESCInfo(mavlink); } + + static constexpr const char *get_name_static() { return "ESC_INFO"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ESC_INFO; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + static constexpr unsigned size_per_batch = MAVLINK_MSG_ID_ESC_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _esc_status_sub.advertised() ? size_per_batch * _number_of_batches : 0; + } + +private: + explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; + uint8_t _number_of_batches{0}; + + bool send() override + { + static constexpr uint8_t batch_size = MAVLINK_MSG_ESC_INFO_FIELD_TEMPERATURE_LEN; + esc_status_s esc_status; + + if (_esc_status_sub.update(&esc_status)) { + mavlink_esc_info_t msg{}; + + msg.time_usec = esc_status.timestamp; + msg.counter = esc_status.counter; + msg.count = esc_status.esc_count; + msg.connection_type = esc_status.esc_connectiontype; + msg.info = esc_status.esc_online_flags; + + // Ceil value of integer division. For 1-4 esc => 1 batch, 5-8 esc => 2 batches etc + _number_of_batches = ceilf((float)esc_status.esc_count / batch_size); + + for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) { + msg.index = batch_number * batch_size; + + for (int esc_index = 0; esc_index < batch_size ; esc_index++) { + msg.failure_flags[esc_index] = esc_status.esc[esc_index].failures; + msg.error_count[esc_index] = esc_status.esc[esc_index].esc_errorcount; + msg.temperature[esc_index] = esc_status.esc[esc_index].esc_temperature; + } + + mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg); + } + + return true; + } + + return false; + } +}; + +#endif // ESC_INFO_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ESC_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ESC_STATUS.hpp new file mode 100644 index 0000000..1e89115 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ESC_STATUS.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ESC_STATUS_HPP +#define ESC_STATUS_HPP + +#include + +class MavlinkStreamESCStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamESCStatus(mavlink); } + + static constexpr const char *get_name_static() { return "ESC_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ESC_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + static constexpr unsigned size_per_batch = MAVLINK_MSG_ID_ESC_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _esc_status_sub.advertised() ? size_per_batch * _number_of_batches : 0; + } + +private: + explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; + uint8_t _number_of_batches{0}; + + bool send() override + { + static constexpr uint8_t batch_size = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN; + esc_status_s esc_status; + + if (_esc_status_sub.update(&esc_status)) { + mavlink_esc_status_t msg{}; + + msg.time_usec = esc_status.timestamp; + + // Ceil value of integer division. For 1-4 esc => 1 batch, 5-8 esc => 2 batches etc + _number_of_batches = ceilf((float)esc_status.esc_count / batch_size); + + for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) { + msg.index = batch_number * batch_size; + + for (int esc_index = 0; esc_index < batch_size ; esc_index++) { + msg.rpm[esc_index] = esc_status.esc[esc_index].esc_rpm; + msg.voltage[esc_index] = esc_status.esc[esc_index].esc_voltage; + msg.current[esc_index] = esc_status.esc[esc_index].esc_current; + } + + mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg); + } + + return true; + } + + return false; + } +}; + +#endif // ESC_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp new file mode 100644 index 0000000..3764940 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ESTIMATOR_STATUS_HPP +#define ESTIMATOR_STATUS_HPP + +#include +#include + +class MavlinkStreamEstimatorStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamEstimatorStatus(mavlink); } + + static constexpr const char *get_name_static() { return "ESTIMATOR_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ESTIMATOR_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _estimator_status_sub.advertised() ? MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + + bool send() override + { + // 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_s est; + + if (_estimator_status_sub.update(&est)) { + mavlink_estimator_status_t est_msg{}; + est_msg.time_usec = est.timestamp; + est_msg.vel_ratio = est.vel_test_ratio; + est_msg.pos_horiz_ratio = est.pos_test_ratio; + est_msg.pos_vert_ratio = est.hgt_test_ratio; + est_msg.mag_ratio = est.mag_test_ratio; + est_msg.hagl_ratio = est.hagl_test_ratio; + est_msg.tas_ratio = est.tas_test_ratio; + est_msg.pos_horiz_accuracy = est.pos_horiz_accuracy; + est_msg.pos_vert_accuracy = est.pos_vert_accuracy; + est_msg.flags = est.solution_status_flags; + mavlink_msg_estimator_status_send_struct(_mavlink->get_channel(), &est_msg); + + return true; + } + + return false; + } +}; + +#endif // ESTIMATOR_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp b/src/prometheus_px4/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp new file mode 100644 index 0000000..b6cf44e --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef EXTENDED_SYS_STATE_HPP +#define EXTENDED_SYS_STATE_HPP + +#include +#include +#include +#include + +class MavlinkStreamExtendedSysState : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamExtendedSysState(mavlink); } + + static constexpr const char *get_name_static() { return "EXTENDED_SYS_STATE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_EXTENDED_SYS_STATE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink) + { + _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; + _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; + } + + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + mavlink_extended_sys_state_t _msg{}; + + bool send() override + { + bool updated = false; + + vehicle_status_s status; + + if (_status_sub.copy(&status)) { + updated = true; + + if (status.is_vtol) { + if (!status.in_transition_mode && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _msg.vtol_state = MAV_VTOL_STATE_MC; + + } else if (!status.in_transition_mode) { + _msg.vtol_state = MAV_VTOL_STATE_FW; + + } else if (status.in_transition_mode && status.in_transition_to_fw) { + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW; + + } else if (status.in_transition_mode) { + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC; + } + } + } + + vehicle_land_detected_s land_detected; + + if (_landed_sub.copy(&land_detected)) { + updated = true; + + if (land_detected.landed) { + _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; + + } else if (!land_detected.landed) { + _msg.landed_state = MAV_LANDED_STATE_IN_AIR; + + vehicle_control_mode_s control_mode; + position_setpoint_triplet_s pos_sp_triplet; + + if (_control_mode_sub.copy(&control_mode) && _pos_sp_triplet_sub.copy(&pos_sp_triplet)) { + if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + _msg.landed_state = MAV_LANDED_STATE_TAKEOFF; + + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _msg.landed_state = MAV_LANDED_STATE_LANDING; + } + } + } + } + } + + if (updated) { + mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg); + } + + return updated; + } +}; + +#endif // EXTENDED_SYS_STATE_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp new file mode 100644 index 0000000..5dd9f88 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef FLIGHT_INFORMATION_HPP +#define FLIGHT_INFORMATION_HPP + +#include + +class MavlinkStreamFlightInformation : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamFlightInformation(mavlink); } + + static constexpr const char *get_name_static() { return "FLIGHT_INFORMATION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_FLIGHT_INFORMATION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamFlightInformation(Mavlink *mavlink) : MavlinkStream(mavlink) + { + _param_com_flight_uuid = param_find("COM_FLIGHT_UUID"); + } + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + param_t _param_com_flight_uuid{PARAM_INVALID}; + + bool send() override + { + vehicle_status_s vehicle_status{}; + + if (_vehicle_status_sub.copy(&vehicle_status) && vehicle_status.timestamp != 0) { + mavlink_flight_information_t flight_info{}; + flight_info.time_boot_ms = hrt_absolute_time() / 1000; + flight_info.arming_time_utc = vehicle_status.armed_time; + flight_info.takeoff_time_utc = vehicle_status.takeoff_time; + + int32_t flight_uuid; + + if (param_get(_param_com_flight_uuid, &flight_uuid) == PX4_OK) { + flight_info.flight_uuid = static_cast(flight_uuid); + } + + mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info); + + return true; + } + + return false; + } +}; + +#endif // FLIGHT_INFORMATION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp new file mode 100644 index 0000000..0d9fe42 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef GIMBAL_DEVICE_ATTITUDE_STATUS_HPP +#define GIMBAL_DEVICE_ATTITUDE_STATUS_HPP + +#include + +class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGimbalDeviceAttitudeStatus(mavlink); } + + static constexpr const char *get_name_static() { return "GIMBAL_DEVICE_ATTITUDE_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_gimbal_device_attitude_status_sub.advertised()) { + return MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + + bool send() override + { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.update(&gimbal_device_attitude_status)) { + mavlink_gimbal_device_attitude_status_t msg{}; + + msg.target_system = gimbal_device_attitude_status.target_system; + msg.target_component = gimbal_device_attitude_status.target_component; + + msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; + + msg.flags = gimbal_device_attitude_status.device_flags; + + msg.q[0] = gimbal_device_attitude_status.q[0]; + msg.q[1] = gimbal_device_attitude_status.q[1]; + msg.q[2] = gimbal_device_attitude_status.q[2]; + msg.q[3] = gimbal_device_attitude_status.q[3]; + + msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; + + msg.failure_flags = gimbal_device_attitude_status.failure_flags; + + mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GIMBAL_DEVICE_ATTITUDE_STATUS diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp new file mode 100644 index 0000000..413e736 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef GIMBAL_DEVICE_SET_ATTITUDE_HPP +#define GIMBAL_DEVICE_SET_ATTITUDE_HPP + +#include + +class MavlinkStreamGimbalDeviceSetAttitude : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGimbalDeviceSetAttitude(mavlink); } + + static constexpr const char *get_name_static() { return "GIMBAL_DEVICE_SET_ATTITUDE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_gimbal_device_set_attitude_sub.advertised()) { + return (MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + + return 0; + } + +private: + explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; + + bool send() override + { + gimbal_device_set_attitude_s gimbal_device_set_attitude; + + if (_gimbal_device_set_attitude_sub.advertised() && _gimbal_device_set_attitude_sub.copy(&gimbal_device_set_attitude)) { + mavlink_gimbal_device_set_attitude_t msg{}; + + msg.target_system = gimbal_device_set_attitude.target_system; + msg.target_component = gimbal_device_set_attitude.target_component; + + msg.flags = gimbal_device_set_attitude.flags; + + msg.q[0] = gimbal_device_set_attitude.q[0]; + msg.q[1] = gimbal_device_set_attitude.q[1]; + msg.q[2] = gimbal_device_set_attitude.q[2]; + msg.q[3] = gimbal_device_set_attitude.q[3]; + + msg.angular_velocity_x = gimbal_device_set_attitude.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_set_attitude.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_set_attitude.angular_velocity_z; + + mavlink_msg_gimbal_device_set_attitude_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GIMBAL_DEVICE_SET_ATTITUDE_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp new file mode 100644 index 0000000..0a84984 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef GIMBAL_MANAGER_INFORMATION_HPP +#define GIMBAL_MANAGER_INFORMATION_HPP + +#include + +class MavlinkStreamGimbalManagerInformation : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGimbalManagerInformation(mavlink); } + + static constexpr const char *get_name_static() { return "GIMBAL_MANAGER_INFORMATION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_gimbal_manager_information_sub.advertised()) { + return MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _gimbal_manager_information_sub{ORB_ID(gimbal_manager_information)}; + + bool send() override + { + gimbal_manager_information_s gimbal_manager_information; + + if (_gimbal_manager_information_sub.advertised() && _gimbal_manager_information_sub.copy(&gimbal_manager_information)) { + // send out gimbal_manager_info with info from gimbal_manager_information + mavlink_gimbal_manager_information_t msg{}; + + msg.time_boot_ms = gimbal_manager_information.timestamp / 1000; + msg.cap_flags = gimbal_manager_information.cap_flags; + msg.gimbal_device_id = 0; + msg.roll_min = gimbal_manager_information.roll_min; + msg.roll_max = gimbal_manager_information.roll_max; + msg.pitch_min = gimbal_manager_information.pitch_min; + msg.pitch_max = gimbal_manager_information.pitch_max; + msg.yaw_min = gimbal_manager_information.yaw_min; + msg.yaw_max = gimbal_manager_information.yaw_max; + + mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GIMBAL_MANAGER_INFORMATION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp new file mode 100644 index 0000000..ecab794 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef GIMBAL_MANAGER_STATUS_HPP +#define GIMBAL_MANAGER_STATUS_HPP + +#include + +class MavlinkStreamGimbalManagerStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGimbalManagerStatus(mavlink); } + + static constexpr const char *get_name_static() { return "GIMBAL_MANAGER_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_gimbal_manager_status_sub.advertised()) { + return MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _gimbal_manager_status_sub{ORB_ID(gimbal_manager_status)}; + + bool send() override + { + gimbal_manager_status_s gimbal_manager_status; + + if (_gimbal_manager_status_sub.advertised() && _gimbal_manager_status_sub.copy(&gimbal_manager_status)) { + mavlink_gimbal_manager_status_t msg{}; + + msg.time_boot_ms = gimbal_manager_status.timestamp / 1000; + msg.flags = gimbal_manager_status.flags; + msg.gimbal_device_id = gimbal_manager_status.gimbal_device_id; + msg.primary_control_sysid = gimbal_manager_status.primary_control_sysid; + msg.primary_control_compid = gimbal_manager_status.primary_control_compid; + msg.secondary_control_sysid = gimbal_manager_status.secondary_control_sysid; + msg.secondary_control_compid = gimbal_manager_status.secondary_control_compid; + + mavlink_msg_gimbal_manager_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GIMBAL_MANAGER_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp new file mode 100644 index 0000000..6d2997f --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef GLOBAL_POSITION_INT_HPP +#define GLOBAL_POSITION_INT_HPP + +#include +#include +#include +#include + +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGlobalPositionInt(mavlink); } + + static constexpr const char *get_name_static() { return "GLOBAL_POSITION_INT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _gpos_sub.advertised() ? MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _home_sub{ORB_ID(home_position)}; + uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; + + bool send() override + { + vehicle_global_position_s gpos; + vehicle_local_position_s lpos; + + if (_gpos_sub.update(&gpos) && _lpos_sub.update(&lpos)) { + + mavlink_global_position_int_t msg{}; + + if (lpos.z_valid && lpos.z_global) { + msg.alt = (-lpos.z + lpos.ref_alt) * 1000.0f; + + } else { + // fall back to baro altitude + vehicle_air_data_s air_data{}; + _air_data_sub.copy(&air_data); + + if (air_data.timestamp > 0) { + msg.alt = air_data.baro_alt_meter * 1000.0f; + } + } + + home_position_s home{}; + _home_sub.copy(&home); + + if ((home.timestamp > 0) && home.valid_alt) { + if (lpos.z_valid) { + msg.relative_alt = -(lpos.z - home.z) * 1000.0f; + + } else { + msg.relative_alt = msg.alt - (home.alt * 1000.0f); + } + + } else { + if (lpos.z_valid) { + msg.relative_alt = -lpos.z * 1000.0f; + } + } + + msg.time_boot_ms = gpos.timestamp / 1000; + msg.lat = gpos.lat * 1e7; + msg.lon = gpos.lon * 1e7; + + msg.vx = lpos.vx * 100.0f; + msg.vy = lpos.vy * 100.0f; + msg.vz = lpos.vz * 100.0f; + + msg.hdg = math::degrees(matrix::wrap_2pi(lpos.heading)) * 100.0f; + + mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GLOBAL_POSITION_INT_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GPS2_RAW.hpp new file mode 100644 index 0000000..7547407 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef GPS2_RAW_HPP +#define GPS2_RAW_HPP + +#include + +class MavlinkStreamGPS2Raw : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPS2Raw(mavlink); } + + static constexpr const char *get_name_static() { return "GPS2_RAW"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS2_RAW; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _sensor_gps_sub.advertised() ? (MAVLINK_MSG_ID_GPS2_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 1}; + + bool send() override + { + sensor_gps_s gps; + + if (_sensor_gps_sub.update(&gps)) { + mavlink_gps2_raw_t msg{}; + + msg.time_usec = gps.timestamp; + msg.fix_type = gps.fix_type; + msg.lat = gps.lat; + msg.lon = gps.lon; + msg.alt = gps.alt; + msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless) + msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless) + + if (PX4_ISFINITE(gps.vel_m_s) && (fabsf(gps.vel_m_s) >= 0.f)) { + msg.vel = gps.vel_m_s * 100.f; // cm/s + + } else { + msg.vel = UINT16_MAX; // If unknown, set to: UINT16_MAX + } + + msg.cog = math::degrees(matrix::wrap_2pi(gps.cog_rad)) * 1e2f; + msg.satellites_visible = gps.satellites_used; + + //msg.dgps_numch = // Number of DGPS satellites + //msg.dgps_age = // Age of DGPS info + + if (PX4_ISFINITE(gps.heading)) { + if (fabsf(gps.heading) < FLT_EPSILON) { + msg.yaw = 36000; // Use 36000 for north. + + } else { + msg.yaw = math::degrees(gps.heading) * 100.f; // centidegrees + } + } + + mavlink_msg_gps2_raw_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GPS2_RAW_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp new file mode 100644 index 0000000..22802b9 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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. + * + ****************************************************************************/ + +#ifndef GPS_GLOBAL_ORIGIN_HPP +#define GPS_GLOBAL_ORIGIN_HPP + +#include + +class MavlinkStreamGpsGlobalOrigin : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGpsGlobalOrigin(mavlink); } + + static constexpr const char *get_name_static() { return "GPS_GLOBAL_ORIGIN"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_vehicle_local_position_sub.advertised()) { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + + bool request_message(float param2, float param3, float param4, float param5, float param6, float param7) override + { + if (_valid) { + _force_next_send = true; + return true; + } + + return false; + } + +private: + explicit MavlinkStreamGpsGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + uint64_t _ref_timestamp{0}; + double _ref_lat{static_cast(NAN)}; + double _ref_lon{static_cast(NAN)}; + float _ref_alt{NAN}; + + bool _valid{false}; + bool _force_next_send{true}; + + bool send() override + { + vehicle_local_position_s vehicle_local_position; + + if (_vehicle_local_position_sub.update(&vehicle_local_position)) { + if (vehicle_local_position.xy_global && vehicle_local_position.z_global) { + + static constexpr double LLA_MIN_DIFF = 0.0000001; // ~11.132 mm at the equator + + if (_force_next_send + || (_ref_timestamp != vehicle_local_position.ref_timestamp) + || (fabs(_ref_lat - vehicle_local_position.ref_lat) > LLA_MIN_DIFF) + || (fabs(_ref_lon - vehicle_local_position.ref_lon) > LLA_MIN_DIFF) + || (fabsf(_ref_alt - vehicle_local_position.ref_alt) > 0.001f)) { + + mavlink_gps_global_origin_t msg{}; + msg.latitude = round(vehicle_local_position.ref_lat * 1e7); // double degree -> int32 degreeE7 + msg.longitude = round(vehicle_local_position.ref_lon * 1e7); // double degree -> int32 degreeE7 + msg.altitude = roundf(vehicle_local_position.ref_alt * 1e3f); // float m -> int32 mm + msg.time_usec = vehicle_local_position.ref_timestamp; // int64 time since system boot + mavlink_msg_gps_global_origin_send_struct(_mavlink->get_channel(), &msg); + + _ref_timestamp = vehicle_local_position.ref_timestamp; + _ref_lat = vehicle_local_position.ref_lat; + _ref_lon = vehicle_local_position.ref_lon; + _ref_alt = vehicle_local_position.ref_alt; + + _force_next_send = false; + _valid = true; + + return true; + } + } + } + + return false; + } +}; + +#endif // GPS_GLOBAL_ORIGIN_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GPS_RAW_INT.hpp new file mode 100644 index 0000000..80c17ec --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef GPS_RAW_INT_HPP +#define GPS_RAW_INT_HPP + +#include + +class MavlinkStreamGPSRawInt : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSRawInt(mavlink); } + + static constexpr const char *get_name_static() { return "GPS_RAW_INT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_RAW_INT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _sensor_gps_sub.advertised() ? (MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 0}; + + bool send() override + { + sensor_gps_s gps; + + if (_sensor_gps_sub.update(&gps)) { + mavlink_gps_raw_int_t msg{}; + + msg.time_usec = gps.timestamp; + msg.fix_type = gps.fix_type; + msg.lat = gps.lat; + msg.lon = gps.lon; + msg.alt = gps.alt; + msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless) + msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless) + + if (PX4_ISFINITE(gps.vel_m_s) && (fabsf(gps.vel_m_s) >= 0.f)) { + msg.vel = gps.vel_m_s * 100.f; // cm/s + + } else { + msg.vel = UINT16_MAX; // If unknown, set to: UINT16_MAX + } + + msg.cog = math::degrees(matrix::wrap_2pi(gps.cog_rad)) * 1e2f; + msg.satellites_visible = gps.satellites_used; + msg.alt_ellipsoid = gps.alt_ellipsoid; + msg.h_acc = gps.eph * 1e3f; // position uncertainty in mm + msg.v_acc = gps.epv * 1e3f; // altitude uncertainty in mm + msg.vel_acc = gps.s_variance_m_s * 1e3f; // speed uncertainty in mm + msg.hdg_acc = math::degrees(gps.c_variance_rad) * 1e5f; // Heading / track uncertainty in degE5 + + if (PX4_ISFINITE(gps.heading)) { + if (fabsf(gps.heading) < FLT_EPSILON) { + msg.yaw = 36000; // Use 36000 for north. + + } else { + msg.yaw = math::degrees(gps.heading) * 100.f; // centidegrees + } + } + + mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GPS_RAW_INT_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/GPS_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/GPS_STATUS.hpp new file mode 100644 index 0000000..82d36f2 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/GPS_STATUS.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef GPS_STATUS_HPP +#define GPS_STATUS_HPP + +#include + +class MavlinkStreamGPSStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSStatus(mavlink); } + + static constexpr const char *get_name_static() { return "GPS_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_STATUS; } + + const char *get_name() const override { return MavlinkStreamGPSStatus::get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _satellite_info_sub.advertised() ? (MAVLINK_MSG_ID_GPS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _satellite_info_sub{ORB_ID(satellite_info)}; + + bool send() override + { + satellite_info_s sat; + + if (_satellite_info_sub.update(&sat)) { + mavlink_gps_status_t msg{}; + + msg.satellites_visible = sat.count; + + size_t sat_count = math::min(static_cast(sat.count), + sizeof(msg.satellite_used) / sizeof(msg.satellite_used[0])); + + for (size_t i = 0; i < sat_count; i++) { + msg.satellite_used[i] = sat.used[i]; + msg.satellite_elevation[i] = sat.elevation[i]; + msg.satellite_azimuth[i] = sat.azimuth[i]; + msg.satellite_snr[i] = sat.snr[i]; + msg.satellite_prn[i] = sat.prn[i]; + } + + mavlink_msg_gps_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // GPS_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/prometheus_px4/src/modules/mavlink/streams/HEARTBEAT.hpp new file mode 100644 index 0000000..ca6f9d6 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef HEARTBEAT_HPP +#define HEARTBEAT_HPP + +#include +#include +#include +#include + +class MavlinkStreamHeartbeat : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHeartbeat(mavlink); } + + static constexpr const char *get_name_static() { return "HEARTBEAT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_HEARTBEAT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + bool const_rate() override { return true; } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _acturator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + + bool send() override + { + if (_mavlink->get_free_tx_buf() >= get_size()) { + // always send the heartbeat, independent of the update status of the topics + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + vehicle_status_flags_s vehicle_status_flags{}; + _vehicle_status_flags_sub.copy(&vehicle_status_flags); + + vehicle_control_mode_s vehicle_control_mode{}; + _vehicle_control_mode_sub.copy(&vehicle_control_mode); + + actuator_armed_s actuator_armed{}; + _acturator_armed_sub.copy(&actuator_armed); + + // uint8_t base_mode (MAV_MODE_FLAG) - System mode bitmap. + uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + if (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) { + base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + if (vehicle_control_mode.flag_control_manual_enabled) { + base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } + + if (vehicle_control_mode.flag_control_attitude_enabled) { + base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + if (vehicle_control_mode.flag_control_auto_enabled) { + base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + } + + + // uint32_t custom_mode - A bitfield for use for autopilot-specific flags + union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)}; + + + // uint8_t system_status (MAV_STATE) - System status flag. + uint8_t system_status = MAV_STATE_UNINIT; + + switch (vehicle_status.arming_state) { + case vehicle_status_s::ARMING_STATE_ARMED: + system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE; + break; + + case vehicle_status_s::ARMING_STATE_STANDBY: + system_status = MAV_STATE_STANDBY; + break; + + case vehicle_status_s::ARMING_STATE_SHUTDOWN: + system_status = MAV_STATE_POWEROFF; + break; + } + + // system_status overrides + if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { + system_status = MAV_STATE_FLIGHT_TERMINATION; + + } else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) { + system_status = MAV_STATE_EMERGENCY; + + } else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL) { + system_status = MAV_STATE_EMERGENCY; + + } else if (vehicle_status_flags.condition_calibration_enabled) { + system_status = MAV_STATE_CALIBRATING; + } + + + mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4, + base_mode, custom_mode.data, system_status); + + return true; + } + + return false; + } +}; + +#endif // HEARTBEAT_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/prometheus_px4/src/modules/mavlink/streams/HIGHRES_IMU.hpp new file mode 100644 index 0000000..5bba947 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef HIGHRES_IMU_HPP +#define HIGHRES_IMU_HPP + +#include +#include +#include +#include +#include +#include +#include + +using matrix::Vector3f; + +class MavlinkStreamHighresIMU : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHighresIMU(mavlink); } + + static constexpr const char *get_name_static() { return "HIGHRES_IMU"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_HIGHRES_IMU; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::SubscriptionMultiArray _vehicle_imu_subs{ORB_ID::vehicle_imu}; + uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; + uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; + uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; + + bool send() override + { + bool updated = false; + + sensor_selection_s sensor_selection{}; + _sensor_selection_sub.copy(&sensor_selection); + + vehicle_imu_s imu; + + for (auto &imu_sub : _vehicle_imu_subs) { + if (imu_sub.update(&imu)) { + if (imu.accel_device_id == sensor_selection.accel_device_id) { + updated = true; + break; + } + } + } + + if (updated) { + uint16_t fields_updated = 0; + + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); // accel + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); // gyro + + vehicle_magnetometer_s magnetometer{}; + + if (_magnetometer_sub.update(&magnetometer)) { + // mark third group dimensions as changed + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + + } else { + _magnetometer_sub.copy(&magnetometer); + } + + // find corresponding estimated sensor bias + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + + Vector3f accel_bias{0.f, 0.f, 0.f}; + Vector3f gyro_bias{0.f, 0.f, 0.f}; + Vector3f mag_bias{0.f, 0.f, 0.f}; + + { + estimator_sensor_bias_s bias; + + if (_estimator_sensor_bias_sub.copy(&bias)) { + if ((bias.accel_device_id != 0) && (bias.accel_device_id == imu.accel_device_id)) { + accel_bias = Vector3f{bias.accel_bias}; + } + + if ((bias.gyro_device_id != 0) && (bias.gyro_device_id == imu.gyro_device_id)) { + gyro_bias = Vector3f{bias.gyro_bias}; + } + + if ((bias.mag_device_id != 0) && (bias.mag_device_id == magnetometer.device_id)) { + mag_bias = Vector3f{bias.mag_bias}; + + } else { + // find primary mag + uORB::SubscriptionMultiArray mag_subs{ORB_ID::vehicle_magnetometer}; + + for (int i = 0; i < mag_subs.size(); i++) { + if (mag_subs[i].advertised() && mag_subs[i].copy(&magnetometer)) { + if (magnetometer.device_id == bias.mag_device_id) { + _magnetometer_sub.ChangeInstance(i); + break; + } + } + + } + } + } + } + + const Vector3f mag = Vector3f{magnetometer.magnetometer_ga} - mag_bias; + + vehicle_air_data_s air_data{}; + + if (_air_data_sub.update(&air_data)) { + /* mark fourth group (baro fields) dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + + } else { + _air_data_sub.copy(&air_data); + } + + differential_pressure_s differential_pressure{}; + + if (_differential_pressure_sub.update(&differential_pressure)) { + /* mark fourth group (dpres field) dimensions as changed */ + fields_updated |= (1 << 10); + + } else { + _differential_pressure_sub.copy(&differential_pressure); + } + + const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f accel = (Vector3f{imu.delta_velocity} * accel_dt_inv) - accel_bias; + + const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; + const Vector3f gyro = (Vector3f{imu.delta_angle} * gyro_dt_inv) - gyro_bias; + + mavlink_highres_imu_t msg{}; + + msg.time_usec = imu.timestamp_sample; + msg.xacc = accel(0); + msg.yacc = accel(1); + msg.zacc = accel(2); + msg.xgyro = gyro(0); + msg.ygyro = gyro(1); + msg.zgyro = gyro(2); + msg.xmag = mag(0); + msg.ymag = mag(1); + msg.zmag = mag(2); + msg.abs_pressure = air_data.baro_pressure_pa; + msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; + msg.pressure_alt = air_data.baro_alt_meter; + msg.temperature = air_data.baro_temp_celcius; + msg.fields_updated = fields_updated; + + mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; +#endif // HIGHRES_IMU_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/prometheus_px4/src/modules/mavlink/streams/HIGH_LATENCY2.hpp new file mode 100644 index 0000000..6953910 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -0,0 +1,661 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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. + * + ****************************************************************************/ + +#ifndef HIGH_LATENCY2_HPP +#define HIGH_LATENCY2_HPP + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class MavlinkStreamHighLatency2 : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHighLatency2(mavlink); } + + static constexpr const char *get_name_static() { return "HIGH_LATENCY2"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_HIGH_LATENCY2; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_HIGH_LATENCY2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() override { return true; } + +private: + explicit MavlinkStreamHighLatency2(Mavlink *mavlink) : + MavlinkStream(mavlink), + _airspeed(SimpleAnalyzer::AVERAGE), + _airspeed_sp(SimpleAnalyzer::AVERAGE), + _climb_rate(SimpleAnalyzer::MAX), + _eph(SimpleAnalyzer::MAX), + _epv(SimpleAnalyzer::MAX), + _groundspeed(SimpleAnalyzer::AVERAGE), + _temperature(SimpleAnalyzer::AVERAGE), + _throttle(SimpleAnalyzer::AVERAGE), + _windspeed(SimpleAnalyzer::AVERAGE) + { + reset_last_sent(); + } + + struct PerBatteryData { + PerBatteryData(uint8_t instance) : subscription(ORB_ID(battery_status), instance) {} + uORB::Subscription subscription; + SimpleAnalyzer analyzer{SimpleAnalyzer::AVERAGE}; + uint64_t timestamp{0}; + bool connected{false}; + }; + + bool send() override + { + const hrt_abstime t = hrt_absolute_time(); + + // only send the struct if transmitting is allowed + // this assures that the stream timer is only reset when actually a message is transmitted + if (_mavlink->should_transmit()) { + mavlink_high_latency2_t msg{}; + set_default_values(msg); + + bool updated = _airspeed.valid(); + updated |= _airspeed_sp.valid(); + + for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { + updated |= _batteries[i].analyzer.valid(); + } + + updated |= _climb_rate.valid(); + updated |= _eph.valid(); + updated |= _epv.valid(); + updated |= _groundspeed.valid(); + updated |= _temperature.valid(); + updated |= _throttle.valid(); + updated |= _windspeed.valid(); + updated |= write_airspeed(&msg); + updated |= write_attitude_sp(&msg); + updated |= write_battery_status(&msg); + updated |= write_estimator_status(&msg); + updated |= write_fw_ctrl_status(&msg); + updated |= write_geofence_result(&msg); + updated |= write_global_position(&msg); + updated |= write_mission_result(&msg); + updated |= write_tecs_status(&msg); + updated |= write_vehicle_status(&msg); + updated |= write_vehicle_status_flags(&msg); + updated |= write_wind(&msg); + + if (updated) { + msg.timestamp = t / 1000; + + msg.type = _mavlink->get_system_type(); + msg.autopilot = MAV_AUTOPILOT_PX4; + + if (_airspeed.valid()) { + _airspeed.get_scaled(msg.airspeed, 5.0f); + } + + if (_airspeed_sp.valid()) { + _airspeed_sp.get_scaled(msg.airspeed_sp, 5.0f); + } + + int lowest = 0; + + for (int i = 1; i < battery_status_s::MAX_INSTANCES; i++) { + const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid(); + const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled( + 100.0f); + + if (battery_connected && battery_is_lowest) { + lowest = i; + } + + } + + if (_batteries[lowest].connected) { + _batteries[lowest].analyzer.get_scaled(msg.battery, 100.0f); + + } else { + msg.battery = -1; + } + + + if (_climb_rate.valid()) { + _climb_rate.get_scaled(msg.climb_rate, 10.0f); + } + + if (_eph.valid()) { + _eph.get_scaled(msg.eph, 10.0f); + } + + if (_epv.valid()) { + _epv.get_scaled(msg.epv, 10.0f); + } + + if (_groundspeed.valid()) { + _groundspeed.get_scaled(msg.groundspeed, 5.0f); + } + + if (_temperature.valid()) { + _temperature.get_scaled(msg.temperature_air, 1.0f); + } + + if (_throttle.valid()) { + _throttle.get_scaled(msg.throttle, 100.0f); + } + + if (_windspeed.valid()) { + _windspeed.get_scaled(msg.windspeed, 5.0f); + } + + reset_analysers(t); + + mavlink_msg_high_latency2_send_struct(_mavlink->get_channel(), &msg); + } + + return updated; + + } else { + // reset the analysers every 60 seconds if nothing should be transmitted + if ((t - _last_reset_time) > 60_s) { + reset_analysers(t); + PX4_DEBUG("Resetting HIGH_LATENCY2 analysers"); + } + + return false; + } + } + + void reset_analysers(const hrt_abstime t) + { + // reset the analyzers + _airspeed.reset(); + _airspeed_sp.reset(); + + for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { + _batteries[i].analyzer.reset(); + } + + _climb_rate.reset(); + _eph.reset(); + _epv.reset(); + _groundspeed.reset(); + _temperature.reset(); + _throttle.reset(); + _windspeed.reset(); + + _last_reset_time = t; + } + + bool write_airspeed(mavlink_high_latency2_t *msg) + { + airspeed_s airspeed; + + if (_airspeed_sub.update(&airspeed)) { + if (airspeed.confidence < 0.95f) { // the same threshold as for the commander + msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; + } + + return true; + } + + return false; + } + + bool write_attitude_sp(mavlink_high_latency2_t *msg) + { + vehicle_attitude_setpoint_s attitude_sp; + + if (_attitude_sp_sub.update(&attitude_sp)) { + msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f); + return true; + } + + return false; + } + + bool write_battery_status(mavlink_high_latency2_t *msg) + { + bool updated = false; + + for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { + battery_status_s battery; + + if (_batteries[i].subscription.update(&battery)) { + updated = true; + _batteries[i].connected = battery.connected; + + if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) { + msg->failure_flags |= HL_FAILURE_FLAG_BATTERY; + } + } + } + + return updated; + } + + bool write_estimator_status(mavlink_high_latency2_t *msg) + { + // 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_s estimator_status; + + if (_estimator_status_sub.update(&estimator_status)) { + if (estimator_status.gps_check_fail_flags > 0 || + estimator_status.filter_fault_flags > 0 || + estimator_status.innovation_check_flags > 0) { + + msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR; + } + + return true; + } + + return false; + } + + bool write_fw_ctrl_status(mavlink_high_latency2_t *msg) + { + position_controller_status_s pos_ctrl_status; + + if (_pos_ctrl_status_sub.update(&pos_ctrl_status)) { + uint16_t target_distance; + convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance); + msg->target_distance = target_distance; + return true; + } + + return false; + } + + bool write_geofence_result(mavlink_high_latency2_t *msg) + { + geofence_result_s geofence; + + if (_geofence_sub.update(&geofence)) { + if (geofence.geofence_violated) { + msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE; + } + + return true; + } + + return false; + } + + bool write_global_position(mavlink_high_latency2_t *msg) + { + vehicle_global_position_s global_pos; + vehicle_local_position_s local_pos; + + if (_global_pos_sub.update(&global_pos) && _local_pos_sub.update(&local_pos)) { + msg->latitude = global_pos.lat * 1e7; + msg->longitude = global_pos.lon * 1e7; + + int16_t altitude = 0; + + if (global_pos.alt > 0) { + convert_limit_safe(global_pos.alt + 0.5f, altitude); + + } else { + convert_limit_safe(global_pos.alt - 0.5f, altitude); + } + + msg->altitude = altitude; + + msg->heading = static_cast(math::degrees(matrix::wrap_2pi(local_pos.heading)) * 0.5f); + + return true; + } + + return false; + } + + bool write_mission_result(mavlink_high_latency2_t *msg) + { + mission_result_s mission_result; + + if (_mission_result_sub.update(&mission_result)) { + msg->wp_num = mission_result.seq_current; + return true; + } + + return false; + } + + bool write_tecs_status(mavlink_high_latency2_t *msg) + { + tecs_status_s tecs_status; + + if (_tecs_status_sub.update(&tecs_status)) { + int16_t target_altitude; + convert_limit_safe(tecs_status.altitude_sp, target_altitude); + msg->target_altitude = target_altitude; + + return true; + } + + return false; + } + + bool write_vehicle_status(mavlink_high_latency2_t *msg) + { + vehicle_status_s status; + + if (_status_sub.update(&status)) { + if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)) { + msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; + } + + if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_ACCEL) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL)) || + ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_ACCEL2) && + !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL2))) { + msg->failure_flags |= HL_FAILURE_FLAG_3D_ACCEL; + } + + if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_GYRO) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO)) || + ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_GYRO2) && + !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO2))) { + msg->failure_flags |= HL_FAILURE_FLAG_3D_GYRO; + } + + if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_MAG) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG)) || + ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_MAG2) && + !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG2))) { + msg->failure_flags |= HL_FAILURE_FLAG_3D_MAG; + } + + if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_TERRAIN) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_TERRAIN)) { + msg->failure_flags |= HL_FAILURE_FLAG_TERRAIN; + } + + if (status.rc_signal_lost) { + msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER; + } + + if (status.engine_failure) { + msg->failure_flags |= HL_FAILURE_FLAG_ENGINE; + } + + if (status.mission_failure) { + msg->failure_flags |= HL_FAILURE_FLAG_MISSION; + } + + // flight mode + union px4_custom_mode custom_mode {get_px4_custom_mode(status.nav_state)}; + msg->custom_mode = custom_mode.custom_mode_hl; + + return true; + } + + return false; + } + + bool write_vehicle_status_flags(mavlink_high_latency2_t *msg) + { + vehicle_status_flags_s status_flags; + + if (_status_flags_sub.update(&status_flags)) { + if (!status_flags.condition_global_position_valid) { //TODO check if there is a better way to get only GPS failure + msg->failure_flags |= HL_FAILURE_FLAG_GPS; + } + + if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_signal_found_once) { + msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK; + } + + return true; + } + + return false; + } + + bool write_wind(mavlink_high_latency2_t *msg) + { + wind_s wind; + + if (_wind_sub.update(&wind)) { + msg->wind_heading = static_cast(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east, + wind.windspeed_north))) * 0.5f); + return true; + } + + return false; + } + + void update_data() override + { + const hrt_abstime t = hrt_absolute_time(); + + if (t > _last_update_time) { + // first order low pass filter for the update rate + _update_rate_filtered = 0.97f * _update_rate_filtered + 0.03f / ((t - _last_update_time) * 1e-6f); + _last_update_time = t; + } + + update_airspeed(); + update_tecs_status(); + update_battery_status(); + update_local_position(); + update_gps(); + update_vehicle_status(); + update_wind(); + } + + void update_airspeed() + { + airspeed_s airspeed; + + if (_airspeed_sub.update(&airspeed)) { + _airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered); + _temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered); + } + } + + + void update_tecs_status() + { + tecs_status_s tecs_status; + + if (_tecs_status_sub.update(&tecs_status)) { + _airspeed_sp.add_value(tecs_status.true_airspeed_sp, _update_rate_filtered); + } + } + + void update_battery_status() + { + battery_status_s battery; + + for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { + if (_batteries[i].subscription.update(&battery)) { + _batteries[i].connected = battery.connected; + _batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered); + } + } + } + + void update_local_position() + { + vehicle_local_position_s local_pos; + + if (_local_pos_sub.update(&local_pos)) { + _climb_rate.add_value(fabsf(local_pos.vz), _update_rate_filtered); + _groundspeed.add_value(sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy), _update_rate_filtered); + } + } + + void update_gps() + { + vehicle_gps_position_s gps; + + if (_gps_sub.update(&gps)) { + _eph.add_value(gps.eph, _update_rate_filtered); + _epv.add_value(gps.epv, _update_rate_filtered); + } + } + + void update_vehicle_status() + { + vehicle_status_s status; + + if (_status_sub.update(&status)) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + actuator_controls_s actuator{}; + + if (status.is_vtol && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_actuator_1_sub.copy(&actuator)) { + _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered); + } + + } else { + if (_actuator_0_sub.copy(&actuator)) { + _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered); + } + } + + } else { + _throttle.add_value(0.0f, _update_rate_filtered); + } + } + } + + void update_wind() + { + wind_s wind; + + if (_wind_sub.update(&wind)) { + _windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east), + _update_rate_filtered); + } + } + + void set_default_values(mavlink_high_latency2_t &msg) const + { + msg.airspeed = 0; + msg.airspeed_sp = 0; + msg.altitude = 0; + msg.autopilot = MAV_AUTOPILOT_ENUM_END; + msg.battery = -1; + msg.climb_rate = 0; + msg.custom0 = INT8_MIN; + msg.custom1 = INT8_MIN; + msg.custom2 = INT8_MIN; + msg.eph = UINT8_MAX; + msg.epv = UINT8_MAX; + msg.failure_flags = 0; + msg.custom_mode = 0; + msg.groundspeed = 0; + msg.heading = 0; + msg.latitude = 0; + msg.longitude = 0; + msg.target_altitude = 0; + msg.target_distance = 0; + msg.target_heading = 0; + msg.temperature_air = INT8_MIN; + msg.throttle = 0; + msg.timestamp = 0; + msg.type = MAV_TYPE_ENUM_END; + msg.wind_heading = 0; + msg.windspeed = 0; + msg.wp_num = UINT16_MAX; + } + + uORB::Subscription _actuator_0_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _actuator_1_sub{ORB_ID(actuator_controls_1)}; + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)}; + uORB::Subscription _geofence_sub{ORB_ID(geofence_result)}; + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)}; + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; + uORB::Subscription _wind_sub{ORB_ID(wind)}; + + SimpleAnalyzer _airspeed; + SimpleAnalyzer _airspeed_sp; + SimpleAnalyzer _climb_rate; + SimpleAnalyzer _eph; + SimpleAnalyzer _epv; + SimpleAnalyzer _groundspeed; + SimpleAnalyzer _temperature; + SimpleAnalyzer _throttle; + SimpleAnalyzer _windspeed; + + hrt_abstime _last_reset_time{0}; + hrt_abstime _last_update_time{0}; + float _update_rate_filtered{0}; + + PerBatteryData _batteries[battery_status_s::MAX_INSTANCES] {0, 1, 2, 3}; +}; + +#endif // HIGH_LATENCY2_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp new file mode 100644 index 0000000..6fff47d --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef HIL_ACTUATOR_CONTROLS_HPP +#define HIL_ACTUATOR_CONTROLS_HPP + +#include +#include +#include + +class MavlinkStreamHILActuatorControls : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHILActuatorControls(mavlink); } + + static constexpr const char *get_name_static() { return "HIL_ACTUATOR_CONTROLS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _act_sub.advertised() ? MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _act_sub{ORB_ID(actuator_outputs)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + + bool send() override + { + actuator_outputs_s act; + + if (_act_sub.update(&act)) { + mavlink_hil_actuator_controls_t msg{}; + msg.time_usec = act.timestamp; + + static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; + + unsigned system_type = _mavlink->get_system_type(); + + /* scale outputs depending on system type */ + if (system_type == MAV_TYPE_QUADROTOR || + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR || + system_type == MAV_TYPE_VTOL_DUOROTOR || + system_type == MAV_TYPE_VTOL_QUADROTOR || + system_type == MAV_TYPE_VTOL_RESERVED2) { + + /* multirotors: set number of rotor outputs depending on type */ + + unsigned n; + + switch (system_type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + case MAV_TYPE_VTOL_DUOROTOR: + n = 2; + break; + + case MAV_TYPE_VTOL_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_VTOL_RESERVED2: + n = 8; + break; + + default: + n = 8; + break; + } + + for (unsigned i = 0; i < 16; i++) { + if (act.output[i] > PWM_DEFAULT_MIN / 2) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ + msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + + } else { + /* scale PWM out 900..2100 us to -1..1 for other channels */ + msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + } + + } else { + /* send 0 when disarmed and for disabled channels */ + msg.controls[i] = 0.0f; + } + } + + } else { + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ + for (unsigned i = 0; i < 16; i++) { + if (act.output[i] > PWM_DEFAULT_MIN / 2) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + } + + } else { + /* set 0 for disabled channels */ + msg.controls[i] = 0.0f; + } + } + } + + // mode (MAV_MODE_FLAG) + msg.mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + vehicle_control_mode_s control_mode; + + if (_vehicle_control_mode_sub.copy(&control_mode)) { + if (control_mode.flag_control_auto_enabled) { + msg.mode |= MAV_MODE_FLAG_AUTO_ENABLED; + } + + if (control_mode.flag_control_manual_enabled) { + msg.mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } + + if (control_mode.flag_control_attitude_enabled) { + msg.mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + } + + vehicle_status_s status; + + if (_vehicle_status_sub.copy(&status)) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + msg.mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { + msg.mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + if (status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + msg.mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + } + + msg.flags = 0; + + mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // HIL_ACTUATOR_CONTROLS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp new file mode 100644 index 0000000..b2dcb5c --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef HIL_STATE_QUATERNION_HPP +#define HIL_STATE_QUATERNION_HPP + +class MavlinkStreamHILStateQuaternion : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHILStateQuaternion(mavlink); } + + static constexpr const char *get_name_static() { return "HIL_STATE_QUATERNION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_HIL_STATE_QUATERNION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_att_sub.advertised() || _gpos_sub.advertised()) { + return MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamHILStateQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity_groundtruth)}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude_groundtruth)}; + uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position_groundtruth)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position_groundtruth)}; + + bool send() override + { + if (_angular_velocity_sub.updated() || _att_sub.updated() || _gpos_sub.updated() || _lpos_sub.updated()) { + vehicle_attitude_s att{}; + _att_sub.copy(&att); + + vehicle_global_position_s gpos{}; + _gpos_sub.copy(&gpos); + + vehicle_local_position_s lpos{}; + _lpos_sub.copy(&lpos); + + vehicle_angular_velocity_s angular_velocity{}; + _angular_velocity_sub.copy(&angular_velocity); + + mavlink_hil_state_quaternion_t msg{}; + + // vehicle_attitude -> hil_state_quaternion + msg.attitude_quaternion[0] = att.q[0]; + msg.attitude_quaternion[1] = att.q[1]; + msg.attitude_quaternion[2] = att.q[2]; + msg.attitude_quaternion[3] = att.q[3]; + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; + + // vehicle_global_position -> hil_state_quaternion + // same units as defined in mavlink/common.xml + msg.lat = gpos.lat * 1e7; + msg.lon = gpos.lon * 1e7; + msg.alt = gpos.alt * 1e3f; + msg.vx = lpos.vx * 1e2f; + msg.vy = lpos.vy * 1e2f; + msg.vz = lpos.vz * 1e2f; + msg.ind_airspeed = 0; + msg.true_airspeed = 0; + msg.xacc = lpos.ax; + msg.yacc = lpos.ay; + msg.zacc = lpos.az; + + mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // HIL_STATE_QUATERNION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/HOME_POSITION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/HOME_POSITION.hpp new file mode 100644 index 0000000..bb67d14 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/HOME_POSITION.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef HOME_POSITION_HPP +#define HOME_POSITION_HPP + +#include + +class MavlinkStreamHomePosition : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHomePosition(mavlink); } + + static constexpr const char *get_name_static() { return "HOME_POSITION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_HOME_POSITION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _home_sub.advertised() ? (MAVLINK_MSG_ID_HOME_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _home_sub{ORB_ID(home_position)}; + + bool send() override + { + // we're sending the GPS home periodically to ensure the + // the GCS does pick it up at one point + home_position_s home; + + if (_home_sub.advertised() && _home_sub.copy(&home)) { + if (home.valid_hpos) { + mavlink_home_position_t msg{}; + + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; + + msg.x = home.x; + msg.y = home.y; + msg.z = home.z; + + matrix::Quatf q(matrix::Eulerf(0.f, 0.f, home.yaw)); + q.copyTo(msg.q); + + msg.approach_x = 0.f; + msg.approach_y = 0.f; + msg.approach_z = 0.f; + + msg.time_usec = home.timestamp; + + mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + } + + return false; + } +}; + +#endif // HOME_POSITION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/LINK_NODE_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/LINK_NODE_STATUS.hpp new file mode 100644 index 0000000..774a4f9 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/LINK_NODE_STATUS.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef LINK_NODE_STATUS_HPP +#define LINK_NODE_STATUS_HPP + +class MavlinkStreamLinkNodeStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamLinkNodeStatus(mavlink); } + + static constexpr const char *get_name_static() { return "LINK_NODE_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_LINK_NODE_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_LINK_NODE_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamLinkNodeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + bool send() override + { + if (_mavlink->get_free_tx_buf() >= get_size()) { + mavlink_link_node_status_t link_node_status{}; + + const telemetry_status_s &tstatus = _mavlink->telemetry_status(); + link_node_status.tx_buf = 0; // % TODO + link_node_status.rx_buf = 0; // % TODO + link_node_status.tx_rate = tstatus.tx_rate_avg; + link_node_status.rx_rate = tstatus.rx_rate_avg; + link_node_status.rx_parse_err = tstatus.rx_parse_errors; + link_node_status.tx_overflows = tstatus.tx_buffer_overruns; + link_node_status.rx_overflows = tstatus.rx_buffer_overruns; + link_node_status.messages_sent = tstatus.tx_message_count; + link_node_status.messages_received = tstatus.rx_message_count; + link_node_status.messages_lost = tstatus.rx_message_lost_count; + + link_node_status.timestamp = hrt_absolute_time(); + + mavlink_msg_link_node_status_send_struct(_mavlink->get_channel(), &link_node_status); + + return true; + } + + return false; + } +}; + +#endif // LINK_NODE_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp b/src/prometheus_px4/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp new file mode 100644 index 0000000..8977999 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef LOCAL_POSITION_NED_HPP +#define LOCAL_POSITION_NED_HPP + +#include + +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamLocalPositionNED(mavlink); } + + static constexpr const char *get_name_static() { return "LOCAL_POSITION_NED"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _lpos_sub.advertised() ? MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + + bool send() override + { + vehicle_local_position_s lpos; + + if (_lpos_sub.update(&lpos)) { + if (lpos.xy_valid && lpos.v_xy_valid) { + mavlink_local_position_ned_t msg{}; + + msg.time_boot_ms = lpos.timestamp / 1000; + msg.x = lpos.x; + msg.y = lpos.y; + msg.z = lpos.z; + msg.vx = lpos.vx; + msg.vy = lpos.vy; + msg.vz = lpos.vz; + + mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + } + + return false; + } +}; + +#endif // LOCAL_POSITION_NED_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/prometheus_px4/src/modules/mavlink/streams/MANUAL_CONTROL.hpp new file mode 100644 index 0000000..3ddb1b4 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef MANUAL_CONTROL_HPP +#define MANUAL_CONTROL_HPP + +#include +#include + +class MavlinkStreamManualControl : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamManualControl(mavlink); } + + static constexpr const char *get_name_static() { return "MANUAL_CONTROL"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_MANUAL_CONTROL; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _manual_control_setpoint_sub.advertised() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : + 0; + } + +private: + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; + + bool send() override + { + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + mavlink_manual_control_t msg{}; + + msg.target = mavlink_system.sysid; + msg.x = manual_control_setpoint.x * 1000; + msg.y = manual_control_setpoint.y * 1000; + msg.z = manual_control_setpoint.z * 1000; + msg.r = manual_control_setpoint.r * 1000; + + manual_control_switches_s manual_control_switches{}; + + if (_manual_control_switches_sub.copy(&manual_control_switches)) { + unsigned shift = 2; + msg.buttons = 0; + msg.buttons |= (manual_control_switches.mode_switch << (shift * 0)); + msg.buttons |= (manual_control_switches.return_switch << (shift * 1)); + msg.buttons |= (manual_control_switches.posctl_switch << (shift * 2)); + msg.buttons |= (manual_control_switches.loiter_switch << (shift * 3)); + msg.buttons |= (manual_control_switches.acro_switch << (shift * 4)); + msg.buttons |= (manual_control_switches.offboard_switch << (shift * 5)); + msg.buttons |= (manual_control_switches.kill_switch << (shift * 6)); + } + + mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // MANUAL_CONTROL diff --git a/src/prometheus_px4/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp new file mode 100644 index 0000000..793766c --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef MOUNT_ORIENTATION_HPP +#define MOUNT_ORIENTATION_HPP + +#include +#include + +class MavlinkStreamMountOrientation : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamMountOrientation(mavlink); } + + static constexpr const char *get_name_static() { return "MOUNT_ORIENTATION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_MOUNT_ORIENTATION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _mount_orientation_sub.advertised() ? MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamMountOrientation(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _mount_orientation_sub{ORB_ID(mount_orientation)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + + bool send() override + { + mount_orientation_s mount_orientation; + + if (_mount_orientation_sub.update(&mount_orientation)) { + mavlink_mount_orientation_t msg{}; + + msg.roll = math::degrees(mount_orientation.attitude_euler_angle[0]); + msg.pitch = math::degrees(mount_orientation.attitude_euler_angle[1]); + msg.yaw = math::degrees(mount_orientation.attitude_euler_angle[2]); + + vehicle_local_position_s lpos{}; + _lpos_sub.copy(&lpos); + msg.yaw_absolute = math::degrees(matrix::wrap_2pi(lpos.heading + mount_orientation.attitude_euler_angle[2])); + + mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // MOUNT_ORIENTATION diff --git a/src/prometheus_px4/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp b/src/prometheus_px4/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp new file mode 100644 index 0000000..9631796 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef NAMED_VALUE_FLOAT_HPP +#define NAMED_VALUE_FLOAT_HPP + +#include + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamNamedValueFloat(mavlink); } + + static constexpr const char *get_name_static() { return "NAMED_VALUE_FLOAT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _debug_key_value_sub.advertised() ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _debug_key_value_sub{ORB_ID(debug_key_value)}; + + bool send() override + { + debug_key_value_s debug; + + if (_debug_key_value_sub.update(&debug)) { + mavlink_named_value_float_t msg{}; + + msg.time_boot_ms = debug.timestamp / 1000ULL; + memcpy(msg.name, debug.key, sizeof(msg.name)); + msg.name[sizeof(msg.name) - 1] = '\0'; // enforce null termination + msg.value = debug.value; + + mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // NAMED_VALUE_FLOAT_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp b/src/prometheus_px4/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp new file mode 100644 index 0000000..6553127 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef NAV_CONTROLLER_OUTPUT_HPP +#define NAV_CONTROLLER_OUTPUT_HPP + +#include +#include + +class MavlinkStreamNavControllerOutput : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamNavControllerOutput(mavlink); } + + static constexpr const char *get_name_static() { return "NAV_CONTROLLER_OUTPUT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _position_controller_status_sub.advertised() ? MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)}; + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; + + bool send() override + { + position_controller_status_s pos_ctrl_status; + + if (_position_controller_status_sub.update(&pos_ctrl_status)) { + + tecs_status_s tecs_status{}; + _tecs_status_sub.copy(&tecs_status); + + mavlink_nav_controller_output_t msg{}; + + msg.nav_roll = math::degrees(pos_ctrl_status.nav_roll); + msg.nav_pitch = math::degrees(pos_ctrl_status.nav_pitch); + msg.nav_bearing = roundf(math::degrees(pos_ctrl_status.nav_bearing)); + msg.target_bearing = roundf(math::degrees(pos_ctrl_status.target_bearing)); + msg.wp_dist = math::constrain(roundf(pos_ctrl_status.wp_dist), 0.f, (float)UINT16_MAX); + msg.xtrack_error = pos_ctrl_status.xtrack_error; + msg.alt_error = tecs_status.altitude_filtered - tecs_status.altitude_sp; + msg.aspd_error = tecs_status.true_airspeed_filtered - tecs_status.true_airspeed_sp; + + mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // NAV_CONTROLLER_OUTPUT_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp b/src/prometheus_px4/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp new file mode 100644 index 0000000..93d936d --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef OBSTACLE_DISTANCE_HPP +#define OBSTACLE_DISTANCE_HPP + +#include + +class MavlinkStreamObstacleDistance : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamObstacleDistance(mavlink); } + + static constexpr const char *get_name_static() { return "OBSTACLE_DISTANCE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OBSTACLE_DISTANCE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _obstacle_distance_fused_sub.advertised() ? (MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _obstacle_distance_fused_sub{ORB_ID(obstacle_distance_fused)}; + + bool send() override + { + obstacle_distance_s obstacle_distance; + + if (_obstacle_distance_fused_sub.update(&obstacle_distance)) { + mavlink_obstacle_distance_t msg{}; + + msg.time_usec = obstacle_distance.timestamp; + msg.sensor_type = obstacle_distance.sensor_type; + memcpy(msg.distances, obstacle_distance.distances, sizeof(msg.distances)); + msg.increment = 0; + msg.min_distance = obstacle_distance.min_distance; + msg.max_distance = obstacle_distance.max_distance; + msg.angle_offset = obstacle_distance.angle_offset; + msg.increment_f = obstacle_distance.increment; + msg.frame = obstacle_distance.frame; + + mavlink_msg_obstacle_distance_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // OBSTACLE_DISTANCE_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ODOMETRY.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ODOMETRY.hpp new file mode 100644 index 0000000..c905dab --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ODOMETRY.hpp @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ODOMETRY_HPP +#define ODOMETRY_HPP + +#include + +class MavlinkStreamOdometry : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOdometry(mavlink); } + + static constexpr const char *get_name_static() { return "ODOMETRY"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ODOMETRY; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_mavlink->odometry_loopback_enabled()) { + return _vodom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + + } else { + return _odom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + } + +private: + explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _odom_sub{ORB_ID(vehicle_odometry)}; + uORB::Subscription _vodom_sub{ORB_ID(vehicle_visual_odometry)}; + + bool send() override + { + vehicle_odometry_s odom; + // check if it is to send visual odometry loopback or not + bool odom_updated = false; + + mavlink_odometry_t msg{}; + + if (_mavlink->odometry_loopback_enabled()) { + odom_updated = _vodom_sub.update(&odom); + + // set the frame_id according to the local frame of the data + if (odom.local_frame == vehicle_odometry_s::LOCAL_FRAME_NED) { + msg.frame_id = MAV_FRAME_LOCAL_NED; + + } else { + msg.frame_id = MAV_FRAME_LOCAL_FRD; + } + + // source: external vision system + msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION; + + } else { + odom_updated = _odom_sub.update(&odom); + + msg.frame_id = MAV_FRAME_LOCAL_NED; + + // source: PX4 estimator + msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; + } + + if (odom_updated) { + msg.time_usec = odom.timestamp_sample; + msg.child_frame_id = MAV_FRAME_BODY_FRD; + + // Current position + msg.x = odom.x; + msg.y = odom.y; + msg.z = odom.z; + + // Current orientation + msg.q[0] = odom.q[0]; + msg.q[1] = odom.q[1]; + msg.q[2] = odom.q[2]; + msg.q[3] = odom.q[3]; + + switch (odom.velocity_frame) { + case vehicle_odometry_s::BODY_FRAME_FRD: + msg.vx = odom.vx; + msg.vy = odom.vy; + msg.vz = odom.vz; + break; + + case vehicle_odometry_s::LOCAL_FRAME_FRD: + case vehicle_odometry_s::LOCAL_FRAME_NED: + // Body frame to local frame + const matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q)); + + // Rotate linear velocity from local to body frame + const matrix::Vector3f linvel_body(R_body_to_local.transpose() * + matrix::Vector3f(odom.vx, odom.vy, odom.vz)); + + msg.vx = linvel_body(0); + msg.vy = linvel_body(1); + msg.vz = linvel_body(2); + break; + } + + // Current body rates + msg.rollspeed = odom.rollspeed; + msg.pitchspeed = odom.pitchspeed; + msg.yawspeed = odom.yawspeed; + + // get the covariance matrix size + + // pose_covariance + static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]); + static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])), + "Odometry Pose Covariance matrix URT array size mismatch"); + + // velocity_covariance + static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]); + static_assert(VEL_URT_SIZE == (sizeof(msg.velocity_covariance) / sizeof(msg.velocity_covariance[0])), + "Odometry Velocity Covariance matrix URT array size mismatch"); + + // copy pose covariances + for (size_t i = 0; i < POS_URT_SIZE; i++) { + msg.pose_covariance[i] = odom.pose_covariance[i]; + } + + // copy velocity covariances + //TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame + for (size_t i = 0; i < VEL_URT_SIZE; i++) { + msg.velocity_covariance[i] = odom.velocity_covariance[i]; + } + + mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ODOMETRY_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp b/src/prometheus_px4/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp new file mode 100644 index 0000000..ebbe35e --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef OPTICAL_FLOW_RAD_HPP +#define OPTICAL_FLOW_RAD_HPP + +#include + +class MavlinkStreamOpticalFlowRad : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpticalFlowRad(mavlink); } + + static constexpr const char *get_name_static() { return "OPTICAL_FLOW_RAD"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } + + const char *get_name() const override { return MavlinkStreamOpticalFlowRad::get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; + + bool send() override + { + optical_flow_s flow; + + if (_optical_flow_sub.update(&flow)) { + mavlink_optical_flow_rad_t msg{}; + + msg.time_usec = flow.timestamp; + msg.sensor_id = flow.sensor_id; + msg.integrated_x = flow.pixel_flow_x_integral; + msg.integrated_y = flow.pixel_flow_y_integral; + msg.integrated_xgyro = flow.gyro_x_rate_integral; + msg.integrated_ygyro = flow.gyro_y_rate_integral; + msg.integrated_zgyro = flow.gyro_z_rate_integral; + msg.distance = flow.ground_distance_m; + msg.quality = flow.quality; + msg.integration_time_us = flow.integration_timespan; + msg.sensor_id = flow.sensor_id; + msg.time_delta_distance_us = flow.time_since_last_sonar_update; + msg.temperature = flow.gyro_temperature; + + mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // OPTICAL_FLOW_RAD_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp new file mode 100644 index 0000000..ddf529d --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef ORBIT_EXECUTION_STATUS_HPP +#define ORBIT_EXECUTION_STATUS_HPP + +#include + +class MavlinkStreamOrbitStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOrbitStatus(mavlink); } + + static constexpr const char *get_name_static() { return "ORBIT_EXECUTION_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _orbit_status_sub.advertised() ? MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _orbit_status_sub{ORB_ID(orbit_status)}; + + bool send() override + { + orbit_status_s orbit_status; + + if (_orbit_status_sub.update(&orbit_status)) { + mavlink_orbit_execution_status_t msg{}; + + msg.time_usec = orbit_status.timestamp; + msg.radius = orbit_status.radius; + msg.frame = orbit_status.frame; + msg.x = orbit_status.x * 1e7; + msg.y = orbit_status.y * 1e7; + msg.z = orbit_status.z; + + mavlink_msg_orbit_execution_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ORBIT_EXECUTION_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/PING.hpp b/src/prometheus_px4/src/modules/mavlink/streams/PING.hpp new file mode 100644 index 0000000..d2a1059 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/PING.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef PING_HPP +#define PING_HPP + +class MavlinkStreamPing : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamPing(mavlink); } + + static constexpr const char *get_name_static() { return "PING"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_PING; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_PING_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() override { return true; } + +private: + explicit MavlinkStreamPing(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uint32_t _sequence{0}; + + bool send() override + { + mavlink_ping_t msg{}; + + msg.time_usec = hrt_absolute_time(); + msg.seq = _sequence++; + msg.target_system = 0; // All systems + msg.target_component = 0; // All components + + mavlink_msg_ping_send_struct(_mavlink->get_channel(), &msg); + + return true; + } +}; + +#endif // PING_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp b/src/prometheus_px4/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp new file mode 100644 index 0000000..f568e2a --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef POSITION_TARGET_GLOBAL_INT_HPP +#define POSITION_TARGET_GLOBAL_INT_HPP + +#include +#include +#include + +class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamPositionTargetGlobalInt(mavlink); } + + static constexpr const char *get_name_static() { return "POSITION_TARGET_GLOBAL_INT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _pos_sp_triplet_sub.advertised() ? MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _lpos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + + bool send() override + { + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); + + if (control_mode.flag_control_position_enabled) { + + position_setpoint_triplet_s pos_sp_triplet{}; + _pos_sp_triplet_sub.copy(&pos_sp_triplet); + + if (pos_sp_triplet.timestamp > 0 && pos_sp_triplet.current.valid + && PX4_ISFINITE(pos_sp_triplet.current.lat) && PX4_ISFINITE(pos_sp_triplet.current.lon)) { + + mavlink_position_target_global_int_t msg{}; + + msg.time_boot_ms = hrt_absolute_time() / 1000; + msg.coordinate_frame = MAV_FRAME_GLOBAL_INT; + msg.lat_int = pos_sp_triplet.current.lat * 1e7; + msg.lon_int = pos_sp_triplet.current.lon * 1e7; + msg.alt = pos_sp_triplet.current.alt; + + vehicle_local_position_setpoint_s lpos_sp; + + if (_lpos_sp_sub.copy(&lpos_sp) && (lpos_sp.timestamp > 0)) { + // velocity + msg.vx = lpos_sp.vx; + msg.vy = lpos_sp.vy; + msg.vz = lpos_sp.vz; + + // acceleration + msg.afx = lpos_sp.acceleration[0]; + msg.afy = lpos_sp.acceleration[1]; + msg.afz = lpos_sp.acceleration[2]; + + // yaw + msg.yaw = lpos_sp.yaw; + msg.yaw_rate = lpos_sp.yawspeed; + } + + mavlink_msg_position_target_global_int_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + } + + return false; + } +}; + +#endif // POSITION_TARGET_GLOBAL_INT_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp b/src/prometheus_px4/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp new file mode 100644 index 0000000..69f3fba --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef POSITION_TARGET_LOCAL_NED_HPP +#define POSITION_TARGET_LOCAL_NED_HPP + +#include + +class MavlinkStreamPositionTargetLocalNed : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamPositionTargetLocalNed(mavlink); } + + static constexpr const char *get_name_static() { return "POSITION_TARGET_LOCAL_NED"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _pos_sp_sub.advertised() ? MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamPositionTargetLocalNed(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; + + bool send() override + { + vehicle_local_position_setpoint_s pos_sp; + + if (_pos_sp_sub.update(&pos_sp)) { + mavlink_position_target_local_ned_t msg{}; + + msg.time_boot_ms = pos_sp.timestamp / 1000; + msg.coordinate_frame = MAV_FRAME_LOCAL_NED; + + // position + if (!PX4_ISFINITE(pos_sp.x)) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_X_IGNORE; + } + + if (!PX4_ISFINITE(pos_sp.y)) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_Y_IGNORE; + } + + if (!PX4_ISFINITE(pos_sp.z)) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_Z_IGNORE; + } + + // velocity + if (!PX4_ISFINITE(pos_sp.vx)) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_VX_IGNORE; + } + + if (!PX4_ISFINITE(pos_sp.vy)) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_VY_IGNORE; + } + + if (!PX4_ISFINITE(pos_sp.vz)) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_VZ_IGNORE; + } + + // acceleration + if (!PX4_ISFINITE(pos_sp.acceleration[0])) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_AX_IGNORE; + } + + if (!PX4_ISFINITE(pos_sp.acceleration[1])) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_AY_IGNORE; + } + + if (!PX4_ISFINITE(pos_sp.acceleration[2])) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_AZ_IGNORE; + } + + // yaw + if (!PX4_ISFINITE(pos_sp.yaw)) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_YAW_IGNORE; + } + + // yaw rate + if (!PX4_ISFINITE(pos_sp.yawspeed)) { + msg.type_mask |= POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; + } + + msg.x = pos_sp.x; + msg.y = pos_sp.y; + msg.z = pos_sp.z; + msg.vx = pos_sp.vx; + msg.vy = pos_sp.vy; + msg.vz = pos_sp.vz; + msg.afx = pos_sp.acceleration[0]; + msg.afy = pos_sp.acceleration[1]; + msg.afz = pos_sp.acceleration[2]; + msg.yaw = pos_sp.yaw; + msg.yaw_rate = pos_sp.yawspeed; + + mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // POSITION_TARGET_LOCAL_NED diff --git a/src/prometheus_px4/src/modules/mavlink/streams/PROTOCOL_VERSION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/PROTOCOL_VERSION.hpp new file mode 100644 index 0000000..a7ee8a0 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/PROTOCOL_VERSION.hpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef PROTOCOL_VERSION_HPP +#define PROTOCOL_VERSION_HPP + +class MavlinkStreamProtocolVersion : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamProtocolVersion(mavlink); } + + static constexpr const char *get_name_static() { return "PROTOCOL_VERSION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_PROTOCOL_VERSION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamProtocolVersion(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + bool send() override + { + _mavlink->send_protocol_version(); + return true; + } +}; + +#endif // PROTOCOL_VERSION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/RAW_RPM.hpp b/src/prometheus_px4/src/modules/mavlink/streams/RAW_RPM.hpp new file mode 100644 index 0000000..ffa4001 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/RAW_RPM.hpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef RAW_RPM_HPP +#define RAW_RPM_HPP + +#include + +class MavlinkStreamRawRpm : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamRawRpm(mavlink); } + + static constexpr const char *get_name_static() { return "RAW_RPM"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_RAW_RPM; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _rpm_sub.advertised() ? (MAVLINK_MSG_ID_RAW_RPM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamRawRpm(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + + bool send() override + { + rpm_s rpm; + + if (_rpm_sub.update(&rpm)) { + mavlink_raw_rpm_t msg{}; + + msg.frequency = rpm.indicated_frequency_rpm; + + mavlink_msg_raw_rpm_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // RAW_RPM_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/RC_CHANNELS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/RC_CHANNELS.hpp new file mode 100644 index 0000000..8689081 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/RC_CHANNELS.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef RC_CHANNELS_HPP +#define RC_CHANNELS_HPP + +#include + +class MavlinkStreamRCChannels : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamRCChannels(mavlink); } + + static constexpr const char *get_name_static() { return "RC_CHANNELS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_RC_CHANNELS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _input_rc_sub.advertised() ? (MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _input_rc_sub{ORB_ID(input_rc)}; + + bool send() override + { + input_rc_s rc; + + if (_input_rc_sub.update(&rc)) { + // send RC channel data and RSSI + mavlink_rc_channels_t msg{}; + + msg.time_boot_ms = rc.timestamp / 1000; + msg.chancount = rc.channel_count; + msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX; + msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX; + msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX; + msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX; + msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX; + msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX; + msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX; + msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX; + msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; + msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; + msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; + msg.rssi = (rc.channel_count > 0) ? rc.rssi : 0; + + mavlink_msg_rc_channels_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; + +#endif // RC_CHANNELS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU.hpp new file mode 100644 index 0000000..efee6f3 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SCALED_IMU_HPP +#define SCALED_IMU_HPP + +#include +#include + +#include +#include + +class MavlinkStreamScaledIMU : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamScaledIMU(mavlink); } + + static constexpr const char *get_name_static() { return "SCALED_IMU"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_IMU; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_vehicle_imu_sub.advertised() || _sensor_mag_sub.advertised()) { + return MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 0}; + uORB::Subscription _sensor_mag_sub{ORB_ID(sensor_mag), 0}; + + bool send() override + { + if (_vehicle_imu_sub.updated() || _sensor_mag_sub.updated()) { + mavlink_scaled_imu_t msg{}; + + vehicle_imu_s imu; + + if (_vehicle_imu_sub.copy(&imu)) { + msg.time_boot_ms = imu.timestamp / 1000; + + // Accelerometer in mG + const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G; + msg.xacc = (int16_t)accel(0); + msg.yacc = (int16_t)accel(1); + msg.zacc = (int16_t)accel(2); + + // Gyroscope in mrad/s + const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; + msg.xgyro = gyro(0); + msg.ygyro = gyro(1); + msg.zgyro = gyro(2); + } + + sensor_mag_s sensor_mag; + + if (_sensor_mag_sub.copy(&sensor_mag)) { + if (msg.time_boot_ms == 0) { + msg.time_boot_ms = sensor_mag.timestamp / 1000; + } + + msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss + msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss + msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss + msg.temperature = sensor_mag.temperature; + } + + mavlink_msg_scaled_imu_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; +#endif /* SCALED_IMU_HPP */ diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU2.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU2.hpp new file mode 100644 index 0000000..d6775bf --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU2.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SCALED_IMU2_HPP +#define SCALED_IMU2_HPP + +#include +#include + +#include +#include + +class MavlinkStreamScaledIMU2 : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamScaledIMU2(mavlink); } + + static constexpr const char *get_name_static() { return "SCALED_IMU2"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_IMU2; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_vehicle_imu_sub.advertised() || _sensor_mag_sub.advertised()) { + return MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 1}; + uORB::Subscription _sensor_mag_sub{ORB_ID(sensor_mag), 1}; + + bool send() override + { + if (_vehicle_imu_sub.updated() || _sensor_mag_sub.updated()) { + mavlink_scaled_imu2_t msg{}; + + vehicle_imu_s imu; + + if (_vehicle_imu_sub.copy(&imu)) { + msg.time_boot_ms = imu.timestamp / 1000; + + // Accelerometer in mG + const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G; + msg.xacc = (int16_t)accel(0); + msg.yacc = (int16_t)accel(1); + msg.zacc = (int16_t)accel(2); + + // Gyroscope in mrad/s + const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; + msg.xgyro = gyro(0); + msg.ygyro = gyro(1); + msg.zgyro = gyro(2); + } + + sensor_mag_s sensor_mag; + + if (_sensor_mag_sub.copy(&sensor_mag)) { + if (msg.time_boot_ms == 0) { + msg.time_boot_ms = sensor_mag.timestamp / 1000; + } + + msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss + msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss + msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss + msg.temperature = sensor_mag.temperature; + } + + mavlink_msg_scaled_imu2_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; +#endif /* SCALED_IMU2_HPP */ diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU3.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU3.hpp new file mode 100644 index 0000000..ea26548 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_IMU3.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SCALED_IMU3_HPP +#define SCALED_IMU3_HPP + +#include +#include + +#include +#include + +class MavlinkStreamScaledIMU3 : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamScaledIMU3(mavlink); } + + static constexpr const char *get_name_static() { return "SCALED_IMU3"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_IMU3; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_vehicle_imu_sub.advertised() || _sensor_mag_sub.advertised()) { + return MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 2}; + uORB::Subscription _sensor_mag_sub{ORB_ID(sensor_mag), 2}; + + bool send() override + { + if (_vehicle_imu_sub.updated() || _sensor_mag_sub.updated()) { + mavlink_scaled_imu3_t msg{}; + + vehicle_imu_s imu; + + if (_vehicle_imu_sub.copy(&imu)) { + msg.time_boot_ms = imu.timestamp / 1000; + + // Accelerometer in mG + const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G; + msg.xacc = (int16_t)accel(0); + msg.yacc = (int16_t)accel(1); + msg.zacc = (int16_t)accel(2); + + // Gyroscope in mrad/s + const float gyro_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; + msg.xgyro = gyro(0); + msg.ygyro = gyro(1); + msg.zgyro = gyro(2); + } + + sensor_mag_s sensor_mag; + + if (_sensor_mag_sub.copy(&sensor_mag)) { + if (msg.time_boot_ms == 0) { + msg.time_boot_ms = sensor_mag.timestamp / 1000; + } + + msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss + msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss + msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss + msg.temperature = sensor_mag.temperature; + } + + mavlink_msg_scaled_imu3_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; +#endif /* SCALED_IMU3_HPP */ diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE.hpp new file mode 100644 index 0000000..3e7263f --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SCALED_PRESSURE_HPP +#define SCALED_PRESSURE_HPP + +#include +#include + +class MavlinkStreamScaledPressure : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamScaledPressure(mavlink); } + + static constexpr const char *get_name_static() { return "SCALED_PRESSURE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_PRESSURE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_sensor_baro_sub.advertised() || _differential_pressure_sub.advertised()) { + return MAVLINK_MSG_ID_SCALED_PRESSURE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 0}; + uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 0}; + + bool send() override + { + if (_sensor_baro_sub.updated() || _differential_pressure_sub.updated()) { + mavlink_scaled_pressure_t msg{}; + + sensor_baro_s sensor_baro; + + if (_sensor_baro_sub.copy(&sensor_baro)) { + msg.time_boot_ms = sensor_baro.timestamp / 1000; + msg.press_abs = sensor_baro.pressure; // millibar to hPa + msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees + } + + differential_pressure_s differential_pressure; + + if (_differential_pressure_sub.copy(&differential_pressure)) { + if (msg.time_boot_ms == 0) { + msg.time_boot_ms = differential_pressure.timestamp / 1000; + } + + msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa + msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees + } + + mavlink_msg_scaled_pressure_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // SCALED_PRESSURE_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp new file mode 100644 index 0000000..e49a17e --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SCALED_PRESSURE2_HPP +#define SCALED_PRESSURE2_HPP + +#include +#include + +class MavlinkStreamScaledPressure2 : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamScaledPressure2(mavlink); } + + static constexpr const char *get_name_static() { return "SCALED_PRESSURE2"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_PRESSURE2; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_sensor_baro_sub.advertised() || _differential_pressure_sub.advertised()) { + return MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamScaledPressure2(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 1}; + uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 1}; + + bool send() override + { + if (_sensor_baro_sub.updated() || _differential_pressure_sub.updated()) { + mavlink_scaled_pressure2_t msg{}; + + sensor_baro_s sensor_baro; + + if (_sensor_baro_sub.copy(&sensor_baro)) { + msg.time_boot_ms = sensor_baro.timestamp / 1000; + msg.press_abs = sensor_baro.pressure; // millibar to hPa + msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees + } + + differential_pressure_s differential_pressure; + + if (_differential_pressure_sub.copy(&differential_pressure)) { + if (msg.time_boot_ms == 0) { + msg.time_boot_ms = differential_pressure.timestamp / 1000; + } + + msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa + msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees + } + + mavlink_msg_scaled_pressure2_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // SCALED_PRESSURE2_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp new file mode 100644 index 0000000..f3fe660 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SCALED_PRESSURE3_HPP +#define SCALED_PRESSURE3_HPP + +#include +#include + +class MavlinkStreamScaledPressure3 : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamScaledPressure3(mavlink); } + + static constexpr const char *get_name_static() { return "SCALED_PRESSURE3"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_PRESSURE3; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_sensor_baro_sub.advertised() || _differential_pressure_sub.advertised()) { + return MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamScaledPressure3(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 2}; + uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 2}; + + bool send() override + { + if (_sensor_baro_sub.updated() || _differential_pressure_sub.updated()) { + mavlink_scaled_pressure3_t msg{}; + + sensor_baro_s sensor_baro; + + if (_sensor_baro_sub.copy(&sensor_baro)) { + msg.time_boot_ms = sensor_baro.timestamp / 1000; + msg.press_abs = sensor_baro.pressure; // millibar to hPa + msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees + } + + differential_pressure_s differential_pressure; + + if (_differential_pressure_sub.copy(&differential_pressure)) { + if (msg.time_boot_ms == 0) { + msg.time_boot_ms = differential_pressure.timestamp / 1000; + } + + msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa + msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees + } + + mavlink_msg_scaled_pressure3_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // SCALED_PRESSURE3_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp new file mode 100644 index 0000000..89b8918 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SERVO_OUTPUT_RAW_HPP +#define SERVO_OUTPUT_RAW_HPP + +#include + +template +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamServoOutputRaw(mavlink); } + + static constexpr const char *get_name_static() + { + switch (N) { + case 0: + return "SERVO_OUTPUT_RAW_0"; + + case 1: + return "SERVO_OUTPUT_RAW_1"; + } + } + + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _act_sub.advertised() ? MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _act_sub{ORB_ID(actuator_outputs), N}; + + bool send() override + { + actuator_outputs_s act; + + if (_act_sub.update(&act)) { + mavlink_servo_output_raw_t msg{}; + + static_assert(sizeof(act.output) / sizeof(act.output[0]) >= 16, "mavlink message requires at least 16 outputs"); + + msg.time_usec = act.timestamp; + msg.port = N; + msg.servo1_raw = act.output[0]; + msg.servo2_raw = act.output[1]; + msg.servo3_raw = act.output[2]; + msg.servo4_raw = act.output[3]; + msg.servo5_raw = act.output[4]; + msg.servo6_raw = act.output[5]; + msg.servo7_raw = act.output[6]; + msg.servo8_raw = act.output[7]; + msg.servo9_raw = act.output[8]; + msg.servo10_raw = act.output[9]; + msg.servo11_raw = act.output[10]; + msg.servo12_raw = act.output[11]; + msg.servo13_raw = act.output[12]; + msg.servo14_raw = act.output[13]; + msg.servo15_raw = act.output[14]; + msg.servo16_raw = act.output[15]; + + mavlink_msg_servo_output_raw_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // SERVO_OUTPUT_RAW_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp new file mode 100644 index 0000000..3476db3 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SMART_BATTERY_INFO_HPP +#define SMART_BATTERY_INFO_HPP + +#include + +class MavlinkStreamSmartBatteryInfo : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSmartBatteryInfo(mavlink); } + + static constexpr const char *get_name_static() { return "SMART_BATTERY_INFO"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SMART_BATTERY_INFO; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return size_per_battery * _battery_status_subs.advertised_count(); + } + +private: + explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + + bool send() override + { + bool updated = false; + + for (auto &battery_sub : _battery_status_subs) { + battery_status_s battery_status; + + if (battery_sub.update(&battery_status)) { + if (battery_status.serial_number == 0) { + // This is not smart battery + continue; + } + + mavlink_smart_battery_info_t msg{}; + + msg.id = battery_status.id - 1; + msg.capacity_full_specification = battery_status.capacity; + msg.capacity_full = (int32_t)((float)(battery_status.state_of_health * battery_status.capacity) / 100.f); + msg.cycle_count = battery_status.cycle_count; + + if (battery_status.manufacture_date) { + uint16_t day = battery_status.manufacture_date % 32; + uint16_t month = (battery_status.manufacture_date >> 5) % 16; + uint16_t year = (80 + (battery_status.manufacture_date >> 9)) % 100; + + //Formatted as 'dd/mm/yy-123456' (maxed 15 + 1 chars) + snprintf(msg.serial_number, sizeof(msg.serial_number), "%d/%d/%d-%d", day, month, year, battery_status.serial_number); + + } else { + snprintf(msg.serial_number, sizeof(msg.serial_number), "%d", battery_status.serial_number); + } + + //msg.device_name = ?? + msg.weight = -1; + msg.discharge_minimum_voltage = -1; + msg.charging_minimum_voltage = -1; + msg.resting_minimum_voltage = -1; + + mavlink_msg_smart_battery_info_send_struct(_mavlink->get_channel(), &msg); + updated = true; + } + } + + return updated; + } +}; + +#endif // SMART_BATTERY_INFO_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/STATUSTEXT.hpp b/src/prometheus_px4/src/modules/mavlink/streams/STATUSTEXT.hpp new file mode 100644 index 0000000..0ee6e45 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/STATUSTEXT.hpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef STATUSTEXT_HPP +#define STATUSTEXT_HPP + +#include + +#include + +class MavlinkStreamStatustext : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamStatustext(mavlink); } + + static constexpr const char *get_name_static() { return "STATUSTEXT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_STATUSTEXT; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _mavlink_log_sub.updated() ? (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + ~MavlinkStreamStatustext() + { + perf_free(_missed_msg_count_perf); + } + + uORB::Subscription _mavlink_log_sub{ORB_ID(mavlink_log)}; + perf_counter_t _missed_msg_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": STATUSTEXT missed messages")}; + uint16_t _id{0}; + + bool send() override + { + if (_mavlink->is_connected()) { + while (_mavlink_log_sub.updated() && (_mavlink->get_free_tx_buf() >= get_size())) { + + const unsigned last_generation = _mavlink_log_sub.get_last_generation(); + + mavlink_log_s mavlink_log; + + if (_mavlink_log_sub.update(&mavlink_log)) { + // don't send stale messages + if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) { + + if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) { + perf_count(_missed_msg_count_perf); + PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(), + perf_event_count(_missed_msg_count_perf)); + } + + mavlink_statustext_t msg{}; + const char *text = mavlink_log.text; + constexpr unsigned max_chunk_size = sizeof(msg.text); + msg.severity = mavlink_log.severity; + msg.chunk_seq = 0; + msg.id = _id++; + unsigned text_size; + + while ((text_size = strlen(text)) > 0) { + unsigned chunk_size = math::min(text_size, max_chunk_size); + + if (chunk_size < max_chunk_size) { + memcpy(&msg.text[0], &text[0], chunk_size); + // pad with zeros + memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size); + + } else { + memcpy(&msg.text[0], &text[0], chunk_size); + } + + mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg); + + if (text_size <= max_chunk_size) { + break; + + } else { + text += max_chunk_size; + } + + msg.chunk_seq += 1; + } + + return true; + } + } + } + } + + return false; + } +}; + +#endif // STATUSTEXT_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/STORAGE_INFORMATION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/STORAGE_INFORMATION.hpp new file mode 100644 index 0000000..2670c2a --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/STORAGE_INFORMATION.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef STORAGE_INFORMATION_HPP +#define STORAGE_INFORMATION_HPP + +#ifdef __PX4_DARWIN +#include +#include +#else +#include +#endif + +#include + +class MavlinkStreamStorageInformation : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamStorageInformation(mavlink); } + + static constexpr const char *get_name_static() { return "STORAGE_INFORMATION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_STORAGE_INFORMATION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool request_message(float param2, float param3, float param4, + float param5, float param6, float param7) override + { + _storage_id = (int)roundf(param2); + return send(); + } +private: + explicit MavlinkStreamStorageInformation(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + int _storage_id{0}; + + bool send() override + { + mavlink_storage_information_t storage_info{}; + const char *microsd_dir = PX4_STORAGEDIR; + + if (_storage_id == 0 || _storage_id == 1) { // request is for all or the first storage + storage_info.storage_id = 1; + + struct statfs statfs_buf; + uint64_t total_bytes = 0; + uint64_t avail_bytes = 0; + + if (statfs(microsd_dir, &statfs_buf) == 0) { + total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize; + avail_bytes = (uint64_t)statfs_buf.f_bavail * statfs_buf.f_bsize; + } + + if (total_bytes == 0) { // on NuttX we get 0 total bytes if no SD card is inserted + storage_info.storage_count = 0; + storage_info.status = 0; // not available + + } else { + storage_info.storage_count = 1; + storage_info.status = 2; // available & formatted + storage_info.total_capacity = total_bytes / 1024. / 1024.; + storage_info.available_capacity = avail_bytes / 1024. / 1024.; + storage_info.used_capacity = (total_bytes - avail_bytes) / 1024. / 1024.; + } + + } else { + return false; // results in MAV_RESULT_DENIED + } + + storage_info.time_boot_ms = hrt_absolute_time() / 1000; + mavlink_msg_storage_information_send_struct(_mavlink->get_channel(), &storage_info); + return true; + } +}; + +#endif // STORAGE_INFORMATION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SYSTEM_TIME.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SYSTEM_TIME.hpp new file mode 100644 index 0000000..167feec --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SYSTEM_TIME.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SYSTEM_TIME_HPP +#define SYSTEM_TIME_HPP + +class MavlinkStreamSystemTime : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSystemTime(mavlink); } + + static constexpr const char *get_name_static() { return "SYSTEM_TIME"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SYSTEM_TIME; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + bool send() override + { + timespec tv; + px4_clock_gettime(CLOCK_REALTIME, &tv); + + mavlink_system_time_t msg{}; + msg.time_boot_ms = hrt_absolute_time() / 1000; + msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + + // If the time is before 2001-01-01, it's probably the default 2000 + // and we don't need to bother sending it because it's definitely wrong. + if (msg.time_unix_usec > 978307200000000) { + mavlink_msg_system_time_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; + +#endif // SYSTEM_TIME_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/SYS_STATUS.hpp new file mode 100644 index 0000000..4fe5d8e --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef SYS_STATUS_HPP +#define SYS_STATUS_HPP + +#include +#include +#include + +class MavlinkStreamSysStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSysStatus(mavlink); } + + static constexpr const char *get_name_static() { return "SYS_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SYS_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + + bool send() override + { + if (_status_sub.updated() || _cpuload_sub.updated() || _battery_status_subs.updated()) { + vehicle_status_s status{}; + _status_sub.copy(&status); + + cpuload_s cpuload{}; + _cpuload_sub.copy(&cpuload); + + battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {}; + + for (int i = 0; i < _battery_status_subs.size(); i++) { + _battery_status_subs[i].copy(&battery_status[i]); + } + + int lowest_battery_index = 0; + + // No battery is connected, select the first group + // Low battery judgment is performed only when the current battery is connected + // When the last cached battery is not connected or the current battery level is lower than the cached battery level, + // the current battery status is replaced with the cached value + for (int i = 0; i < _battery_status_subs.size(); i++) { + if (battery_status[i].connected && ((!battery_status[lowest_battery_index].connected) + || (battery_status[i].remaining < battery_status[lowest_battery_index].remaining))) { + lowest_battery_index = i; + } + } + + mavlink_sys_status_t msg{}; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + + msg.load = cpuload.load * 1000.0f; + + // TODO: Determine what data should be put here when there are multiple batteries. + // Right now, it uses the lowest battery. This is a safety decision, because if a client is only checking + // one battery using this message, it should be the lowest. + // In the future, this should somehow determine the "main" battery, or use the "type" field of BATTERY_STATUS + // to determine which battery is more important at a given time. + const battery_status_s &lowest_battery = battery_status[lowest_battery_index]; + + if (lowest_battery.connected) { + msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; + msg.current_battery = lowest_battery.current_filtered_a * 100.0f; + msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); + + } else { + msg.voltage_battery = UINT16_MAX; + msg.current_battery = -1; + msg.battery_remaining = -1; + } + + mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; + +#endif // SYS_STATUS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/TIMESYNC.hpp b/src/prometheus_px4/src/modules/mavlink/streams/TIMESYNC.hpp new file mode 100644 index 0000000..0c52a8b --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/TIMESYNC.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef TIMESYNC_HPP +#define TIMESYNC_HPP + +#include + +class MavlinkStreamTimesync : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTimesync(mavlink); } + + static constexpr const char *get_name_static() { return "TIMESYNC"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TIMESYNC; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + bool send() override + { + mavlink_timesync_t msg{}; + + msg.tc1 = 0; + msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds + + mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &msg); + + return true; + } +}; + +#endif // TIMESYNC_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp b/src/prometheus_px4/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp new file mode 100644 index 0000000..174c633 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP +#define TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP + +#include + +class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTrajectoryRepresentationWaypoints(mavlink); } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + static constexpr const char *get_name_static() { return "TRAJECTORY_REPRESENTATION_WAYPOINTS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; } + + unsigned get_size() override + { + if (_traj_wp_avoidance_sub.advertised()) { + return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)}; + + bool send() override + { + vehicle_trajectory_waypoint_s traj_wp_avoidance_desired; + + if (_traj_wp_avoidance_sub.update(&traj_wp_avoidance_desired)) { + mavlink_trajectory_representation_waypoints_t msg{}; + + msg.time_usec = traj_wp_avoidance_desired.timestamp; + int number_valid_points = 0; + + for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { + msg.pos_x[i] = traj_wp_avoidance_desired.waypoints[i].position[0]; + msg.pos_y[i] = traj_wp_avoidance_desired.waypoints[i].position[1]; + msg.pos_z[i] = traj_wp_avoidance_desired.waypoints[i].position[2]; + + msg.vel_x[i] = traj_wp_avoidance_desired.waypoints[i].velocity[0]; + msg.vel_y[i] = traj_wp_avoidance_desired.waypoints[i].velocity[1]; + msg.vel_z[i] = traj_wp_avoidance_desired.waypoints[i].velocity[2]; + + msg.acc_x[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[0]; + msg.acc_y[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[1]; + msg.acc_z[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[2]; + + msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw; + msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed; + + switch (traj_wp_avoidance_desired.waypoints[i].type) { + case position_setpoint_s::SETPOINT_TYPE_TAKEOFF: + msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; + break; + + case position_setpoint_s::SETPOINT_TYPE_LOITER: + msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; + break; + + case position_setpoint_s::SETPOINT_TYPE_LAND: + msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LAND; + break; + + default: + msg.command[i] = UINT16_MAX; + } + + if (traj_wp_avoidance_desired.waypoints[i].point_valid) { + number_valid_points++; + } + + } + + msg.valid_points = number_valid_points; + + mavlink_msg_trajectory_representation_waypoints_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp new file mode 100644 index 0000000..f441bd6 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef UTM_GLOBAL_POSITION_HPP +#define UTM_GLOBAL_POSITION_HPP + +#include +#include +#include +#include +#include + +class MavlinkStreamUTMGlobalPosition : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamUTMGlobalPosition(mavlink); } + + static constexpr const char *get_name_static() { return "UTM_GLOBAL_POSITION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_UTM_GLOBAL_POSITION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + bool const_rate() override { return true; } + + unsigned get_size() override + { + return _global_pos_sub.advertised() ? MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; + + bool send() override + { + vehicle_global_position_s global_pos; + + if (_global_pos_sub.update(&global_pos)) { + mavlink_utm_global_position_t msg{}; + + // Compute Unix epoch and set time field + timespec tv; + px4_clock_gettime(CLOCK_REALTIME, &tv); + uint64_t unix_epoch = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + + // If the time is before 2001-01-01, it's probably the default 2000 + if (unix_epoch > 978307200000000) { + msg.time = unix_epoch; + msg.flags |= UTM_DATA_AVAIL_FLAGS_TIME_VALID; + } + +#ifndef BOARD_HAS_NO_UUID + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + static_assert(sizeof(px4_guid_t) == sizeof(msg.uas_id), "GUID byte length mismatch"); + memcpy(&msg.uas_id, &px4_guid, sizeof(msg.uas_id)); + msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; +#else + // TODO Fill ID with something reasonable + memset(&msg.uas_id[0], 0, sizeof(msg.uas_id)); +#endif /* BOARD_HAS_NO_UUID */ + + // Handle global position + msg.lat = global_pos.lat * 1e7; + msg.lon = global_pos.lon * 1e7; + msg.alt = global_pos.alt_ellipsoid * 1000.f; + + msg.h_acc = global_pos.eph * 1000.f; + msg.v_acc = global_pos.epv * 1000.f; + + msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; + msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; + + // Handle local position + vehicle_local_position_s local_pos; + + if (_local_pos_sub.copy(&local_pos)) { + float evh = 0.f; + float evv = 0.f; + + if (local_pos.v_xy_valid) { + msg.vx = local_pos.vx * 100.f; + msg.vy = local_pos.vy * 100.f; + evh = local_pos.evh; + msg.flags |= UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE; + } + + if (local_pos.v_z_valid) { + msg.vz = local_pos.vz * 100.f; + evv = local_pos.evv; + msg.flags |= UTM_DATA_AVAIL_FLAGS_VERTICAL_VELO_AVAILABLE; + } + + msg.vel_acc = sqrtf(evh * evh + evv * evv) * 100.f; + + if (local_pos.dist_bottom_valid) { + msg.relative_alt = local_pos.dist_bottom * 1000.f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE; + } + } + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + bool vehicle_in_auto_mode = (vehicle_status.timestamp > 0) + && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + + // Handle next waypoint if it is valid + position_setpoint_triplet_s position_setpoint_triplet; + + if (vehicle_in_auto_mode && _position_setpoint_triplet_sub.copy(&position_setpoint_triplet)) { + if (position_setpoint_triplet.current.valid) { + msg.next_lat = position_setpoint_triplet.current.lat * 1e7; + msg.next_lon = position_setpoint_triplet.current.lon * 1e7; + // HACK We assume that the offset between AMSL and WGS84 is constant between the current + // vehicle position and the the target waypoint. + msg.next_alt = (position_setpoint_triplet.current.alt + (global_pos.alt_ellipsoid - global_pos.alt)) * 1000.f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE; + } + } + + // Handle flight state + vehicle_land_detected_s land_detected{}; + _land_detected_sub.copy(&land_detected); + + if (vehicle_status.timestamp > 0 && land_detected.timestamp > 0 + && vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + if (land_detected.landed) { + msg.flight_state |= UTM_FLIGHT_STATE_GROUND; + + } else { + msg.flight_state |= UTM_FLIGHT_STATE_AIRBORNE; + } + + } else { + msg.flight_state |= UTM_FLIGHT_STATE_UNKNOWN; + } + + msg.update_rate = 0; // Data driven mode + + mavlink_msg_utm_global_position_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // UTM_GLOBAL_POSITION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/VFR_HUD.hpp b/src/prometheus_px4/src/modules/mavlink/streams/VFR_HUD.hpp new file mode 100644 index 0000000..b5a038a --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/VFR_HUD.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef VFR_HUD_HPP +#define VFR_HUD_HPP + +#include +#include +#include +#include +#include + +class MavlinkStreamVFRHUD : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamVFRHUD(mavlink); } + + static constexpr const char *get_name_static() { return "VFR_HUD"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_VFR_HUD; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_lpos_sub.advertised() || _airspeed_validated_sub.advertised()) { + return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _act0_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _act1_sub{ORB_ID(actuator_controls_1)}; + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; + + bool send() override + { + if (_lpos_sub.updated() || _airspeed_validated_sub.updated()) { + + vehicle_local_position_s lpos{}; + _lpos_sub.copy(&lpos); + + actuator_armed_s armed{}; + _armed_sub.copy(&armed); + + airspeed_validated_s airspeed_validated{}; + _airspeed_validated_sub.copy(&airspeed_validated); + + mavlink_vfr_hud_t msg{}; + msg.airspeed = airspeed_validated.calibrated_airspeed_m_s; + msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy); + msg.heading = math::degrees(matrix::wrap_2pi(lpos.heading)); + + if (armed.armed) { + actuator_controls_s act0{}; + actuator_controls_s act1{}; + _act0_sub.copy(&act0); + _act1_sub.copy(&act1); + + // VFR_HUD throttle should only be used for operator feedback. + // VTOLs switch between actuator_controls_0 and actuator_controls_1. During transition there isn't a + // a single throttle value, but this should still be a useful heuristic for operator awareness. + // + // Use ACTUATOR_CONTROL_TARGET if accurate states are needed. + msg.throttle = 100 * math::max( + act0.control[actuator_controls_s::INDEX_THROTTLE], + act1.control[actuator_controls_s::INDEX_THROTTLE]); + + } else { + msg.throttle = 0.0f; + } + + if (lpos.z_valid && lpos.z_global) { + /* use local position estimate */ + msg.alt = -lpos.z + lpos.ref_alt; + + } else { + vehicle_air_data_s air_data{}; + _air_data_sub.copy(&air_data); + + /* fall back to baro altitude */ + if (air_data.timestamp > 0) { + msg.alt = air_data.baro_alt_meter; + } + } + + if (lpos.v_z_valid) { + msg.climb = -lpos.vz; + } + + mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // VFR_HUD_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/VIBRATION.hpp b/src/prometheus_px4/src/modules/mavlink/streams/VIBRATION.hpp new file mode 100644 index 0000000..0ab42f8 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/VIBRATION.hpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef VIBRATION_HPP +#define VIBRATION_HPP + +#include +#include + +class MavlinkStreamVibration : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamVibration(mavlink); } + + static constexpr const char *get_name_static() { return "VIBRATION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_VIBRATION; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_sensor_selection_sub.advertised() && _vehicle_imu_status_subs.advertised()) { + return MAVLINK_MSG_ID_VIBRATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamVibration(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; + uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; + + bool send() override + { + if (_vehicle_imu_status_subs.updated()) { + mavlink_vibration_t msg{}; + msg.time_usec = hrt_absolute_time(); + + // VIBRATION usage not to mavlink spec, this is our current usage. + // vibration_x : Primary gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) + // vibration_y : Primary gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) + // vibration_z : Primary accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) + + sensor_selection_s sensor_selection{}; + _sensor_selection_sub.copy(&sensor_selection); + + // primary accel high frequency vibration metric + if (sensor_selection.accel_device_id != 0) { + for (auto &x : _vehicle_imu_status_subs) { + vehicle_imu_status_s status; + + if (x.copy(&status)) { + if (status.accel_device_id == sensor_selection.accel_device_id) { + msg.vibration_x = status.gyro_coning_vibration; + msg.vibration_y = status.gyro_vibration_metric; + msg.vibration_z = status.accel_vibration_metric; + break; + } + } + } + } + + // accel 0, 1, 2 cumulative clipping + for (int i = 0; i < math::min(static_cast(3), _vehicle_imu_status_subs.size()); i++) { + vehicle_imu_status_s status; + + if (_vehicle_imu_status_subs[i].copy(&status)) { + + const uint32_t clipping = status.accel_clipping[0] + status.accel_clipping[1] + status.accel_clipping[2]; + + switch (i) { + case 0: + msg.clipping_0 = clipping; + break; + + case 1: + msg.clipping_1 = clipping; + break; + + case 2: + msg.clipping_2 = clipping; + break; + } + } + } + + mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // VIBRATION_HPP diff --git a/src/prometheus_px4/src/modules/mavlink/streams/WIND_COV.hpp b/src/prometheus_px4/src/modules/mavlink/streams/WIND_COV.hpp new file mode 100644 index 0000000..8cd5550 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/streams/WIND_COV.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef WIND_COV_HPP +#define WIND_COV_HPP + +#include +#include + +class MavlinkStreamWindCov : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamWindCov(mavlink); } + + static constexpr const char *get_name_static() { return "WIND_COV"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_WIND_COV; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _wind_sub.advertised() ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _wind_sub{ORB_ID(wind)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + + bool send() override + { + wind_s wind; + + if (_wind_sub.update(&wind)) { + mavlink_wind_cov_t msg{}; + + msg.time_usec = wind.timestamp; + + msg.wind_x = wind.windspeed_north; + msg.wind_y = wind.windspeed_east; + msg.wind_z = 0.0f; + + msg.var_horiz = wind.variance_north + wind.variance_east; + msg.var_vert = 0.0f; + + vehicle_local_position_s lpos{}; + _local_pos_sub.copy(&lpos); + msg.wind_alt = (lpos.z_valid && lpos.z_global) ? (-lpos.z + lpos.ref_alt) : (float)NAN; + + msg.horiz_accuracy = 0.0f; + msg.vert_accuracy = 0.0f; + + mavlink_msg_wind_cov_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // WIND_COV diff --git a/src/prometheus_px4/src/modules/mavlink/timestamped_list.h b/src/prometheus_px4/src/modules/mavlink/timestamped_list.h new file mode 100644 index 0000000..67f1df2 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/timestamped_list.h @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 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 timestamped list.h + * Fixed size list with timestamps. + * + * The list has a fixed size that is set at instantiation and is based + * on timestamps. If a new value is put into a full list, the oldest value + * is overwritten. + * + * @author Julian Oes + */ + +#pragma once + +#include + +/** + * @class TimestampedList + */ +template +class TimestampedList +{ +public: + TimestampedList(int num_items) + { + _list = new item_s[num_items]; + _list_len = num_items; + } + + ~TimestampedList() + { + delete[] _list; + } + + /** + * Insert a value into the list, overwrite the oldest entry if full. + */ + void put(const T &new_value) + { + hrt_abstime now = hrt_absolute_time(); + + // Insert it wherever there is a free space. + for (int i = 0; i < _list_len; ++i) { + if (_list[i].timestamp_us == 0) { + _list[i].timestamp_us = now; + _list[i].value = new_value; + return; + } + } + + // Find oldest entry. + int oldest_i = 0; + + for (int i = 1; i < _list_len; ++i) { + if (_list[i].timestamp_us < _list[oldest_i].timestamp_us) { + oldest_i = i; + } + } + + // And overwrite oldest. + _list[oldest_i].timestamp_us = now; + _list[oldest_i].value = new_value; + } + + /** + * Before iterating using get_next(), reset to start. + */ + void reset_to_start() + { + _current_i = -1; + } + + /** + * Iterate through all active values (not sorted). + * Return nullptr if at end of list. + * + * This is basically a poor man's iterator. + */ + T *get_next() + { + // Increment first, then leave it until called again. + ++_current_i; + + for (int i = _current_i; i < _list_len; ++i) { + if (_list[i].timestamp_us != 0) { + _current_i = i; + return &_list[i].value; + } + } + + return nullptr; + } + + /** + * Disable the last item that we have gotten. + */ + void drop_current() + { + if (_current_i < _list_len) { + _list[_current_i].timestamp_us = 0; + } + } + + /** + * Update the timestamp of the item we have gotten. + */ + void update_current() + { + if (_current_i < _list_len) { + _list[_current_i].timestamp = hrt_absolute_time(); + } + } + + /* do not allow copying or assigning this class */ + TimestampedList(const TimestampedList &) = delete; + TimestampedList operator=(const TimestampedList &) = delete; + +private: + struct item_s { + hrt_abstime timestamp_us = 0; // 0 signals inactive. + T value; + }; + + item_s *_list = nullptr; + int _list_len = 0; + int _current_i = -1; +}; diff --git a/src/prometheus_px4/src/modules/mavlink/tune_publisher.cpp b/src/prometheus_px4/src/modules/mavlink/tune_publisher.cpp new file mode 100644 index 0000000..baba588 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/tune_publisher.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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 "tune_publisher.h" +#include "string.h" +#include + +void TunePublisher::set_tune_string(const char *tune, const hrt_abstime &now) +{ + // The tune string needs to be 0 terminated. + const unsigned tune_len = strlen(tune); + + // We don't expect the tune string to be longer than what can come in via MAVLink including 0 termination. + if (tune_len >= MAX_TUNE_LEN) { + PX4_ERR("Tune string too long."); + return; + } + + strncpy(_tune_buffer, tune, MAX_TUNE_LEN); + + _tunes.set_string(_tune_buffer, tune_control_s::VOLUME_LEVEL_DEFAULT); + + _next_publish_time = now; +} + + +void TunePublisher::publish_next_tune(const hrt_abstime now) +{ + if (_next_publish_time == 0) { + // Nothing to play. + return; + } + + if (now < _next_publish_time) { + // Too early, try again later. + return; + } + + unsigned frequency; + unsigned duration; + unsigned silence; + uint8_t volume; + + if (_tunes.get_next_note(frequency, duration, silence, volume) == Tunes::Status::Continue) { + tune_control_s tune_control{}; + tune_control.frequency = static_cast(frequency); + tune_control.duration = static_cast(duration); + tune_control.silence = static_cast(silence); + tune_control.volume = static_cast(volume); + tune_control.timestamp = hrt_absolute_time(); + _tune_control_pub.publish(tune_control); + + _next_publish_time = now + duration + silence; + + } else { + // We're done, let's reset. + _next_publish_time = 0; + } +} diff --git a/src/prometheus_px4/src/modules/mavlink/tune_publisher.h b/src/prometheus_px4/src/modules/mavlink/tune_publisher.h new file mode 100644 index 0000000..ea1ff44 --- /dev/null +++ b/src/prometheus_px4/src/modules/mavlink/tune_publisher.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 +#include +#include + + +class TunePublisher +{ +public: + void set_tune_string(const char *tune, const hrt_abstime &now); + void publish_next_tune(const hrt_abstime now); + +private: + static constexpr unsigned MAX_TUNE_LEN {248}; + + Tunes _tunes {}; + char _tune_buffer[MAX_TUNE_LEN] {0}; + hrt_abstime _next_publish_time {0}; + + uORB::Publication _tune_control_pub{ORB_ID(tune_control)}; +}; diff --git a/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp new file mode 100644 index 0000000..859b863 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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 AttitudeControl.cpp + */ + +#include + +#include + +using namespace matrix; + +void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight) +{ + _proportional_gain = proportional_gain; + _yaw_w = math::constrain(yaw_weight, 0.f, 1.f); + + // compensate for the effect of the yaw weight rescaling the output + if (_yaw_w > 1e-4f) { + _proportional_gain(2) /= _yaw_w; + } +} + +matrix::Vector3f AttitudeControl::update(const Quatf &q) const +{ + Quatf qd = _attitude_setpoint_q; + + // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch + const Vector3f e_z = q.dcm_z(); + const Vector3f e_z_d = qd.dcm_z(); + Quatf qd_red(e_z, e_z_d); + + if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) { + // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction, + // full attitude control anyways generates no yaw input and directly takes the combination of + // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. + qd_red = qd; + + } else { + // transform rotation from current to desired thrust vector into a world frame reduced desired attitude + qd_red *= q; + } + + // mix full and reduced desired attitude + Quatf q_mix = qd_red.inversed() * qd; + q_mix.canonicalize(); + // catch numerical problems with the domain of acosf and asinf + q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); + q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); + qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3)))); + + // quaternion attitude control law, qe is rotation from q to qd + const Quatf qe = q.inversed() * qd; + + // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) + // also taking care of the antipodal unit quaternion ambiguity + const Vector3f eq = 2.f * qe.canonical().imag(); + + // calculate angular rates setpoint + matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); + + // Feed forward the yaw setpoint rate. + // yawspeed_setpoint is the feed forward commanded rotation around the world z-axis, + // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame). + // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed) + // and multiply it by the yaw setpoint rate (yawspeed_setpoint). + // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame + // such that it can be added to the rates setpoint. + if (is_finite(_yawspeed_setpoint)) { + rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; + } + + // limit rates + for (int i = 0; i < 3; i++) { + rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i)); + } + + return rate_setpoint; +} diff --git a/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp new file mode 100644 index 0000000..70980e2 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 AttitudeControl.hpp + * + * A quaternion based attitude controller. + * + * @author Matthias Grob + * + * Publication documenting the implemented Quaternion Attitude Control: + * Nonlinear Quadrocopter Attitude Control (2013) + * by Dario Brescianini, Markus Hehn and Raffaello D'Andrea + * Institute for Dynamic Systems and Control (IDSC), ETH Zurich + * + * https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + */ + +#pragma once + +#include +#include + +class AttitudeControl +{ +public: + AttitudeControl() = default; + ~AttitudeControl() = default; + + /** + * Set proportional attitude control gain + * @param proportional_gain 3D vector containing gains for roll, pitch, yaw + * @param yaw_weight A fraction [0,1] deprioritizing yaw compared to roll and pitch + */ + void setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight); + + /** + * Set hard limit for output rate setpoints + * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw + */ + void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; } + + /** + * Set a new attitude setpoint replacing the one tracked before + * @param qd desired vehicle attitude setpoint + * @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame + */ + void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _attitude_setpoint_q.normalize(); _yawspeed_setpoint = yawspeed_setpoint; } + + /** + * Adjust last known attitude setpoint by a delta rotation + * Optional use to avoid glitches when attitude estimate reference e.g. heading changes. + * @param q_delta delta rotation to apply + */ + void adaptAttitudeSetpoint(const matrix::Quatf &q_delta) { _attitude_setpoint_q = q_delta * _attitude_setpoint_q; } + + /** + * Run one control loop cycle calculation + * @param q estimation of the current vehicle attitude unit quaternion + * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller + */ + matrix::Vector3f update(const matrix::Quatf &q) const; + +private: + matrix::Vector3f _proportional_gain; + matrix::Vector3f _rate_limit; + float _yaw_w{0.f}; ///< yaw weight [0,1] to deprioritize caompared to roll and pitch + + matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control + float _yawspeed_setpoint{0.f}; ///< latest known yawspeed feed-forward setpoint +}; diff --git a/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp new file mode 100644 index 0000000..2803686 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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 +#include +#include + +using namespace matrix; + +TEST(AttitudeControlTest, AllZeroCase) +{ + AttitudeControl attitude_control; + Vector3f rate_setpoint = attitude_control.update(Quatf()); + EXPECT_EQ(rate_setpoint, Vector3f()); +} + +class AttitudeControlConvergenceTest : public ::testing::Test +{ +public: + AttitudeControlConvergenceTest() + { + _attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f), .4f); + _attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f)); + } + + void checkConvergence() + { + int i; // need function scope to check how many steps + Vector3f rate_setpoint(1000.f, 1000.f, 1000.f); + + _attitude_control.setAttitudeSetpoint(_quat_goal, 0.f); + + for (i = 100; i > 0; i--) { + // run attitude control to get rate setpoints + const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state); + // rotate the simulated state quaternion according to the rate setpoint + _quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new)); + _quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem + + // expect the error and hence also the output to get smaller with each iteration + if (rate_setpoint_new.norm() >= rate_setpoint.norm()) { + break; + } + + rate_setpoint = rate_setpoint_new; + } + + EXPECT_EQ(_quat_state.canonical(), _quat_goal.canonical()); + // it shouldn't have taken longer than an iteration timeout to converge + EXPECT_GT(i, 0); + } + + AttitudeControl _attitude_control; + Quatf _quat_state; + Quatf _quat_goal; +}; + +TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence) +{ + const int inputs = 8; + + const Quatf QArray[inputs] = { + Quatf(), + Quatf(0, 1, 0, 0), + Quatf(0, 0, 1, 0), + Quatf(0, 0, 0, 1), + Quatf(0.698f, 0.024f, -0.681f, -0.220f), + Quatf(-0.820f, -0.313f, 0.225f, -0.423f), + Quatf(0.599f, -0.172f, 0.755f, -0.204f), + Quatf(0.216f, -0.662f, 0.290f, -0.656f) + }; + + for (int i = 0; i < inputs; i++) { + for (int j = 0; j < inputs; j++) { + printf("--- Input combination: %d %d\n", i, j); + _quat_state = QArray[i]; + _quat_goal = QArray[j]; + _quat_state.normalize(); + _quat_goal.normalize(); + checkConvergence(); + } + } +} + +TEST(AttitudeControlTest, YawWeightScaling) +{ + // GIVEN: default tuning and pure yaw turn command + AttitudeControl attitude_control; + const float yaw_gain = 2.8f; + const float yaw_sp = .1f; + Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)); + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), .4f); + attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f)); + attitude_control.setAttitudeSetpoint(pure_yaw_attitude, 0.f); + + // WHEN: we run one iteration of the controller + Vector3f rate_setpoint = attitude_control.update(Quatf()); + + // THEN: no actuation in roll, pitch + EXPECT_EQ(Vector2f(rate_setpoint), Vector2f()); + // THEN: actuation error * gain in yaw + EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f); + + // GIVEN: additional corner case of zero yaw weight + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), 0.f); + // WHEN: we run one iteration of the controller + rate_setpoint = attitude_control.update(Quatf()); + // THEN: no actuation (also no NAN) + EXPECT_EQ(rate_setpoint, Vector3f()); +} diff --git a/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt b/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt new file mode 100644 index 0000000..4ff754e --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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(AttitudeControl + AttitudeControl.cpp + AttitudeControl.hpp +) +target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(AttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl) diff --git a/src/prometheus_px4/src/modules/mc_att_control/CMakeLists.txt b/src/prometheus_px4/src/modules/mc_att_control/CMakeLists.txt new file mode 100644 index 0000000..abd0f4f --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_att_control/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015-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(AttitudeControl) + +px4_add_module( + MODULE modules__mc_att_control + MAIN mc_att_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + mc_att_control_main.cpp + mc_att_control.hpp + DEPENDS + AttitudeControl + mathlib + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/mc_att_control/mc_att_control.hpp b/src/prometheus_px4/src/modules/mc_att_control/mc_att_control.hpp new file mode 100644 index 0000000..c8cc197 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_att_control/mc_att_control.hpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 // Airmode +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace time_literals; + +/** + * Multicopter attitude control app start / stop handling function + */ +extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); + +class MulticopterAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + MulticopterAttitudeControl(bool vtol = false); + ~MulticopterAttitudeControl() 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; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + float throttle_curve(float throttle_stick_input); + + /** + * Generate & publish an attitude setpoint from stick inputs + */ + void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp); + + AttitudeControl _attitude_control; ///< class for attitude control calculations + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + + uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; + + uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _vehicle_attitude_setpoint_pub; + + struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + AlphaFilter _man_x_input_filter; + AlphaFilter _man_y_input_filter; + + hrt_abstime _last_run{0}; + + bool _landed{true}; + bool _reset_yaw_sp{true}; + bool _vehicle_type_rotary_wing{true}; + bool _vtol{false}; + bool _vtol_tailsitter{false}; + bool _vtol_in_transition_mode{false}; + + uint8_t _quat_reset_counter{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mc_roll_p, + (ParamFloat) _param_mc_pitch_p, + (ParamFloat) _param_mc_yaw_p, + (ParamFloat) _param_mc_yaw_weight, + + (ParamFloat) _param_mc_rollrate_max, + (ParamFloat) _param_mc_pitchrate_max, + (ParamFloat) _param_mc_yawrate_max, + + (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ + + /* Stabilized mode params */ + (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ + (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ + (ParamFloat) _param_mpc_thr_max, /**< maximum throttle for stabilized */ + (ParamFloat) + _param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */ + (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ + + (ParamInt) _param_mc_airmode, + (ParamFloat) _param_mc_man_tilt_tau + ) +}; + diff --git a/src/prometheus_px4/src/modules/mc_att_control/mc_att_control_main.cpp b/src/prometheus_px4/src/modules/mc_att_control/mc_att_control_main.cpp new file mode 100644 index 0000000..fe5cf9e --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_att_control/mc_att_control_main.cpp @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mc_att_control_main.cpp + * Multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + * @author Sander Smeets + * @author Matthias Grob + * @author Beat Küng + * + */ + +#include "mc_att_control.hpp" + +#include +#include +#include + +using namespace matrix; + +MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), + _vtol(vtol) +{ + if (_vtol) { + int32_t vt_type = -1; + + if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) { + _vtol_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + } + } + + parameters_updated(); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +MulticopterAttitudeControl::init() +{ + if (!_vehicle_attitude_sub.registerCallback()) { + PX4_ERR("vehicle_attitude callback registration failed!"); + return false; + } + + return true; +} + +void +MulticopterAttitudeControl::parameters_updated() +{ + // Store some of the parameters in a more convenient way & precompute often-used values + _attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get()), + _param_mc_yaw_weight.get()); + + // angular rate limits + using math::radians; + _attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()), + radians(_param_mc_yawrate_max.get()))); + + _man_tilt_max = math::radians(_param_mpc_man_tilt_max.get()); +} + +float +MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) +{ + // 油门曲线映射：根据参数调整油门响应曲线，支持悬停油门居中或线性映射，用于无人机姿态控制中的油门输入处理 + const float throttle_min = _landed ? 0.0f : _param_mpc_manthr_min.get(); + + // throttle_stick_input is in range [0, 1] + switch (_param_mpc_thr_curve.get()) { + case 1: // no rescaling to hover throttle + return throttle_min + throttle_stick_input * (_param_mpc_thr_max.get() - throttle_min); + + default: // 0 or other: rescale to hover throttle at 0.5 stick + return math::gradual3(throttle_stick_input, + 0.f, .5f, 1.f, + throttle_min, _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()); + } +} + +void +MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp) +{ + vehicle_attitude_setpoint_s attitude_setpoint{}; + const float yaw = Eulerf(q).psi(); + + /* reset yaw setpoint to current position if needed */ + if (reset_yaw_sp) { + _man_yaw_sp = yaw; + + } else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f + || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { + + const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); + attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; + _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); + } + + /* + * Input mapping for roll & pitch setpoints + * ---------------------------------------- + * We control the following 2 angles: + * - tilt angle, given by sqrt(x*x + y*y) + * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion + * + * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick + * points to, and changes of the stick input are linear. + */ + _man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); + _man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); + _man_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max); + _man_y_input_filter.update(_manual_control_setpoint.y * _man_tilt_max); + const float x = _man_x_input_filter.getState(); + const float y = _man_y_input_filter.getState(); + + // we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane + Vector2f v = Vector2f(y, -x); + float v_norm = v.norm(); // the norm of v defines the tilt angle + + if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle + v *= _man_tilt_max / v_norm; + } + + Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f); + Eulerf euler_sp = q_sp_rpy; + attitude_setpoint.roll_body = euler_sp(0); + attitude_setpoint.pitch_body = euler_sp(1); + // The axis angle can change the yaw as well (noticeable at higher tilt angles). + // This is the formula by how much the yaw changes: + // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) + // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). + attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2); + + /* modify roll/pitch only if we're a VTOL */ + if (_vtol) { + // Construct attitude setpoint rotation matrix. Modify the setpoints for roll + // and pitch such that they reflect the user's intention even if a large yaw error + // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix + // from the pure euler angle setpoints will lead to unexpected attitude behaviour from + // the user's view as the euler angle sequence uses the yaw setpoint and not the current + // heading of the vehicle. + // However there's also a coupling effect that causes oscillations for fast roll/pitch changes + // at higher tilt angles, so we want to avoid using this on multicopters. + // The effect of that can be seen with: + // - roll/pitch into one direction, keep it fixed (at high angle) + // - apply a fast yaw rotation + // - look at the roll and pitch angles: they should stay pretty much the same as when not yawing + + // calculate our current yaw error + float yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw); + + // compute the vector obtained by rotating a z unit vector by the rotation + // given by the roll and pitch commands of the user + Vector3f zB = {0.0f, 0.0f, 1.0f}; + Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f); + Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB; + + // transform the vector into a new frame which is rotated around the z axis + // by the current yaw error. this vector defines the desired tilt when we look + // into the direction of the desired heading + Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error); + z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; + + // use the formula z_roll_pitch_sp = R_tilt * [0;0;1] + // R_tilt is computed from_euler; only true if cos(roll) not equal zero + // -> valid if roll is not +-pi/2; + attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1)); + attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2)); + } + + /* copy quaternion setpoint to attitude setpoint topic */ + Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); + q_sp.copyTo(attitude_setpoint.q_d); + + attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f)); + attitude_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); +} + +void +MulticopterAttitudeControl::Run() +{ + if (should_exit()) { + _vehicle_attitude_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(¶m_update); + + updateParams(); + parameters_updated(); + } + + // run controller on attitude updates + vehicle_attitude_s v_att; + + if (_vehicle_attitude_sub.update(&v_att)) { + + // Check for new attitude setpoint + if (_vehicle_attitude_setpoint_sub.updated()) { + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + _vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint); + _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); + _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); + } + + // Check for a heading reset + if (_quat_reset_counter != v_att.quat_reset_counter) { + const Quatf delta_q_reset(v_att.delta_q_reset); + // for stabilized attitude generation only extract the heading change from the delta quaternion + _man_yaw_sp += Eulerf(delta_q_reset).psi(); + _attitude_control.adaptAttitudeSetpoint(delta_q_reset); + + _quat_reset_counter = v_att.quat_reset_counter; + } + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((v_att.timestamp - _last_run) * 1e-6f), 0.0002f, 0.02f); + _last_run = v_att.timestamp; + + /* check for updates in other topics */ + _manual_control_setpoint_sub.update(&_manual_control_setpoint); + _v_control_mode_sub.update(&_v_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; + } + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _vehicle_type_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + _vtol = vehicle_status.is_vtol; + _vtol_in_transition_mode = vehicle_status.in_transition_mode; + } + } + + bool attitude_setpoint_generated = false; + + const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode); + + // vehicle is a tailsitter in transition mode + const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode); + + bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); + + if (run_att_ctrl) { + + const Quatf q{v_att.q}; + + // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode + if (_v_control_mode.flag_control_manual_enabled && + !_v_control_mode.flag_control_altitude_enabled && + !_v_control_mode.flag_control_velocity_enabled && + !_v_control_mode.flag_control_position_enabled) { + + generate_attitude_setpoint(q, dt, _reset_yaw_sp); + attitude_setpoint_generated = true; + + } else { + _man_x_input_filter.reset(0.f); + _man_y_input_filter.reset(0.f); + } + + Vector3f rates_sp = _attitude_control.update(q); + + // publish rate setpoint + vehicle_rates_setpoint_s v_rates_sp{}; + v_rates_sp.roll = rates_sp(0); + v_rates_sp.pitch = rates_sp(1); + v_rates_sp.yaw = rates_sp(2); + _thrust_setpoint_body.copyTo(v_rates_sp.thrust_body); + v_rates_sp.timestamp = hrt_absolute_time(); + + _v_rates_sp_pub.publish(v_rates_sp); + } + + // reset yaw setpoint during transitions, tailsitter.cpp generates + // attitude setpoint for the transition + _reset_yaw_sp = !attitude_setpoint_generated || _landed || (_vtol && _vtol_in_transition_mode); + } + + perf_end(_loop_perf); +} + +int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol); + + 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 MulticopterAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the multicopter attitude controller. It takes attitude +setpoints (`vehicle_attitude_setpoint`) as inputs and outputs a rate setpoint. + +The controller has a P loop for angular error + +Publication documenting the implemented Quaternion Attitude Control: +Nonlinear Quadrocopter Attitude Control (2013) +by Dario Brescianini, Markus Hehn and Raffaello D'Andrea +Institute for Dynamic Systems and Control (IDSC), ETH Zurich + +https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int mc_att_control_main(int argc, char *argv[]) +{ + return MulticopterAttitudeControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/mc_att_control/mc_att_control_params.c b/src/prometheus_px4/src/modules/mc_att_control/mc_att_control_params.c new file mode 100644 index 0000000..f45e856 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_att_control/mc_att_control_params.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 mc_att_control_params.c + * Parameters for multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 12 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 12 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 5 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f); + +/** + * Yaw weight + * + * A fraction [0,1] deprioritizing yaw compared to roll and pitch in non-linear attitude control. + * Deprioritizing yaw is necessary because multicopters have much less control authority + * in yaw compared to the other axes and it makes sense because yaw is not critical for + * stable hovering or 3D navigation. + * + * For yaw control tuning use MC_YAW_P. This ratio has no inpact on the yaw gain. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAW_WEIGHT, 0.4f); + +/** + * Max roll rate + * + * Limit for roll rate in manual and auto modes (except acro). + * Has effect for large rotations in autonomous mode, to avoid large control + * output and mixer saturation. + * + * This is not only limited by the vehicle's properties, but also by the maximum + * measurement rate of the gyro. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 220.0f); + +/** + * Max pitch rate + * + * Limit for pitch rate in manual and auto modes (except acro). + * Has effect for large rotations in autonomous mode, to avoid large control + * output and mixer saturation. + * + * This is not only limited by the vehicle's properties, but also by the maximum + * measurement rate of the gyro. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); + +/** + * Max yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f); + +/** + * Manual tilt input filter time constant + * + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MC_MAN_TILT_TAU, 0.0f); diff --git a/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/CMakeLists.txt b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/CMakeLists.txt new file mode 100644 index 0000000..9504a18 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(zero_order_hover_thrust_ekf + zero_order_hover_thrust_ekf.cpp + zero_order_hover_thrust_ekf.hpp +) + +px4_add_module( + MODULE modules__mc_hover_thrust_estimator + MAIN mc_hover_thrust_estimator + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + MulticopterHoverThrustEstimator.cpp + MulticopterHoverThrustEstimator.hpp + DEPENDS + hysteresis + mathlib + px4_work_queue + zero_order_hover_thrust_ekf +) + +px4_add_unit_gtest(SRC zero_order_hover_thrust_ekf_test.cpp LINKLIBS zero_order_hover_thrust_ekf) diff --git a/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp new file mode 100644 index 0000000..4074916 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -0,0 +1,301 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hover_thrust_estimator.cpp + * + * @author Mathieu Bresciani + */ + +#include "MulticopterHoverThrustEstimator.hpp" + +#include + +using namespace time_literals; + +MulticopterHoverThrustEstimator::MulticopterHoverThrustEstimator() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + _valid_hysteresis.set_hysteresis_time_from(false, 2_s); + updateParams(); + reset(); +} + +MulticopterHoverThrustEstimator::~MulticopterHoverThrustEstimator() +{ + perf_free(_cycle_perf); +} + +bool MulticopterHoverThrustEstimator::init() +{ + if (!_vehicle_local_position_sub.registerCallback()) { + PX4_ERR("vehicle_local_position_setpoint callback registration failed!"); + return false; + } + + return true; +} + +void MulticopterHoverThrustEstimator::reset() +{ + _hover_thrust_ekf.setHoverThrust(_param_mpc_thr_hover.get()); + _hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get()); + _hover_thrust_ekf.resetAccelNoise(); +} + +void MulticopterHoverThrustEstimator::updateParams() +{ + const float ht_err_init_prev = _param_hte_ht_err_init.get(); + ModuleParams::updateParams(); + + _hover_thrust_ekf.setProcessNoiseStdDev(_param_hte_ht_noise.get()); + + if (fabsf(_param_hte_ht_err_init.get() - ht_err_init_prev) > FLT_EPSILON) { + _hover_thrust_ekf.setHoverThrustStdDev(_param_hte_ht_err_init.get()); + } + + _hover_thrust_ekf.setAccelInnovGate(_param_hte_acc_gate.get()); +} + +void MulticopterHoverThrustEstimator::Run() +{ + if (should_exit()) { + _vehicle_local_position_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + 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; + + if (_landed) { + _in_air = false; + } + } + } + + if (!_vehicle_local_position_sub.updated()) { + return; + } + + vehicle_local_position_s local_pos{}; + + if (_vehicle_local_position_sub.copy(&local_pos)) { + // This is only necessary because the landed + // flag of the land detector does not guarantee that + // the vehicle does not touch the ground anymore. + // There is no check for the dist_bottom validity as + // this value is always good enough after takeoff for + // this use case. + // TODO: improve the landed flag + if (!_landed) { + if (local_pos.dist_bottom > 1.f) { + _in_air = true; + } + } + } + + // new local position setpoint needed every iteration + if (!_vehicle_local_position_setpoint_sub.updated()) { + return; + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } + + perf_begin(_cycle_perf); + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + } + + const float dt = (local_pos.timestamp - _timestamp_last) * 1e-6f; + _timestamp_last = local_pos.timestamp; + + if (_armed && _in_air && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) { + + _hover_thrust_ekf.predict(dt); + + vehicle_local_position_setpoint_s local_pos_sp; + + if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) { + if (PX4_ISFINITE(local_pos_sp.thrust[2])) { + // Inform the hover thrust estimator about the measured vertical + // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) + // Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects + if (fabsf(local_pos.vz) < 2.f) { + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); + } + + bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); + + // The test ratio does not need to pass all the time to have a valid estimate + if (!_valid) { + valid = valid && (_hover_thrust_ekf.getInnovationTestRatio() < 1.f); + } + + _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); + _valid = _valid_hysteresis.get_state(); + + publishStatus(local_pos.timestamp); + } + } + + } else { + _valid_hysteresis.set_state_and_update(false, hrt_absolute_time()); + + if (!_armed) { + reset(); + } + + if (_valid) { + // only publish a single message to invalidate + publishInvalidStatus(); + + _valid = false; + } + } + + perf_end(_cycle_perf); +} + +void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime ×tamp_sample) +{ + hover_thrust_estimate_s status_msg{}; + + status_msg.timestamp_sample = timestamp_sample; + + status_msg.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate(); + status_msg.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar(); + + status_msg.accel_innov = _hover_thrust_ekf.getInnovation(); + status_msg.accel_innov_var = _hover_thrust_ekf.getInnovationVar(); + status_msg.accel_innov_test_ratio = _hover_thrust_ekf.getInnovationTestRatio(); + status_msg.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar(); + + status_msg.valid = _valid; + + status_msg.timestamp = hrt_absolute_time(); + + _hover_thrust_ekf_pub.publish(status_msg); +} + +void MulticopterHoverThrustEstimator::publishInvalidStatus() +{ + hover_thrust_estimate_s status_msg{}; + + status_msg.hover_thrust = NAN; + status_msg.hover_thrust_var = NAN; + status_msg.accel_innov = NAN; + status_msg.accel_innov_var = NAN; + status_msg.accel_innov_test_ratio = NAN; + status_msg.accel_noise_var = NAN; + + status_msg.timestamp = hrt_absolute_time(); + + _hover_thrust_ekf_pub.publish(status_msg); +} + +int MulticopterHoverThrustEstimator::task_spawn(int argc, char *argv[]) +{ + MulticopterHoverThrustEstimator *instance = new MulticopterHoverThrustEstimator(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int MulticopterHoverThrustEstimator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterHoverThrustEstimator::print_status() +{ + perf_print_counter(_cycle_perf); + + return 0; +} + +int MulticopterHoverThrustEstimator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_hover_thrust_estimator", "estimator"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_hover_thrust_estimator_main(int argc, char *argv[]) +{ + return MulticopterHoverThrustEstimator::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp new file mode 100644 index 0000000..6f6280e --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hover_thrust_estimator.hpp + * @brief Interface class for a hover thrust estimator + * Convention is positive thrust, hover thrust and acceleration UP + * + * @author Mathieu Bresciani + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "zero_order_hover_thrust_ekf.hpp" + +using namespace time_literals; + +class MulticopterHoverThrustEstimator : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + MulticopterHoverThrustEstimator(); + ~MulticopterHoverThrustEstimator() 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(); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + void Run() override; + void updateParams() override; + + void reset(); + + void publishStatus(const hrt_abstime ×tamp_sample); + void publishInvalidStatus(); + + ZeroOrderHoverThrustEkf _hover_thrust_ekf{}; + + uORB::Publication _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)}; + + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + + hrt_abstime _timestamp_last{0}; + + bool _armed{false}; + bool _landed{false}; + bool _in_air{false}; + + bool _valid{false}; + + systemlib::Hysteresis _valid_hysteresis{false}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_hte_ht_noise, + (ParamFloat) _param_hte_acc_gate, + (ParamFloat) _param_hte_ht_err_init, + (ParamFloat) _param_mpc_thr_hover + ) +}; diff --git a/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c new file mode 100644 index 0000000..47d7f6e --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hover_thrust_estimator_params.c + * + * Parameters used by the hover thrust estimator + * + * @author Mathieu Bresciani + */ + +/** + * Hover thrust process noise + * + * Reduce to make the hover thrust estimate + * more stable, increase if the real hover thrust + * is expected to change quickly over time. + * + * @decimal 4 + * @min 0.0001 + * @max 1.0 + * @unit normalized_thrust/s + * @group Hover Thrust Estimator + */ +PARAM_DEFINE_FLOAT(HTE_HT_NOISE, 0.0036); + +/** + * Gate size for acceleration fusion + * + * Sets the number of standard deviations used + * by the innovation consistency test. + * + * @decimal 1 + * @min 1.0 + * @max 10.0 + * @unit SD + * @group Hover Thrust Estimator + */ +PARAM_DEFINE_FLOAT(HTE_ACC_GATE, 3.0); + +/** + * 1-sigma initial hover thrust uncertainty + * + * Sets the number of standard deviations used + * by the innovation consistency test. + * + * @decimal 3 + * @min 0.0 + * @max 1.0 + * @unit normalized_thrust + * @group Hover Thrust Estimator + */ +PARAM_DEFINE_FLOAT(HTE_HT_ERR_INIT, 0.1); diff --git a/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp new file mode 100644 index 0000000..94c13ca --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * 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 zero_order_thrust_ekf.cpp + * + * @author Mathieu Bresciani + */ + +#include "zero_order_hover_thrust_ekf.hpp" + +using matrix::sign; + +void ZeroOrderHoverThrustEkf::predict(const float dt) +{ + // State is constant + // Predict state covariance only + _state_var += _process_var * dt * dt; + _dt = dt; +} + +void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust) +{ + const float H = computeH(thrust); + const float innov_var = computeInnovVar(H); + const float innov = computeInnov(acc_z, thrust); + const float K = computeKalmanGain(H, innov_var); + const float innov_test_ratio = computeInnovTestRatio(innov, innov_var); + + float residual = innov; + + if (isTestRatioPassing(innov_test_ratio)) { + updateState(K, innov); + updateStateCovariance(K, H); + residual = computeInnov(acc_z, thrust); // residual != innovation since the hover thrust changed + + } else if (isLargeOffsetDetected()) { + // Rejecting all the measurements for some time, + // it means that the hover thrust suddenly changed or that the EKF + // is diverging. To recover, we bump the state variance + bumpStateVariance(); + } + + const float signed_innov_test_ratio = sign(innov) * innov_test_ratio; + updateLpf(residual, signed_innov_test_ratio); + updateMeasurementNoise(residual, H); + + // save for logging + _innov = innov; + _innov_var = innov_var; + _innov_test_ratio = innov_test_ratio; +} + +inline float ZeroOrderHoverThrustEkf::computeH(const float thrust) const +{ + return -CONSTANTS_ONE_G * thrust / (_hover_thr * _hover_thr); +} + +inline float ZeroOrderHoverThrustEkf::computeInnovVar(const float H) const +{ + const float R = _acc_var; + const float P = _state_var; + return math::max(H * P * H + R, R); +} + +float ZeroOrderHoverThrustEkf::computeInnov(const float acc_z, const float thrust) const +{ + const float predicted_acc_z = computePredictedAccZ(thrust); + return acc_z - predicted_acc_z; +} + +float ZeroOrderHoverThrustEkf::computePredictedAccZ(const float thrust) const +{ + return CONSTANTS_ONE_G * thrust / _hover_thr - CONSTANTS_ONE_G; +} + +inline float ZeroOrderHoverThrustEkf::computeKalmanGain(const float H, const float innov_var) const +{ + return _state_var * H / innov_var; +} + +inline float ZeroOrderHoverThrustEkf::computeInnovTestRatio(const float innov, const float innov_var) const +{ + return innov * innov / (_gate_size * _gate_size * innov_var); +} + +inline bool ZeroOrderHoverThrustEkf::isTestRatioPassing(const float innov_test_ratio) const +{ + return innov_test_ratio < 1.f; +} + +inline void ZeroOrderHoverThrustEkf::updateState(const float K, const float innov) +{ + _hover_thr = math::constrain(_hover_thr + K * innov, 0.1f, 0.9f); +} + +inline void ZeroOrderHoverThrustEkf::updateStateCovariance(const float K, const float H) +{ + _state_var = math::constrain((1.f - K * H) * _state_var, 1e-10f, 1.f); +} + +inline bool ZeroOrderHoverThrustEkf::isLargeOffsetDetected() const +{ + return fabsf(_signed_innov_test_ratio_lpf) > 0.2f; +} + +inline void ZeroOrderHoverThrustEkf::bumpStateVariance() +{ + _state_var += 1e3f * _process_var * _dt * _dt; +} + +inline void ZeroOrderHoverThrustEkf::updateLpf(const float residual, const float signed_innov_test_ratio) +{ + const float alpha = _dt / (_lpf_time_constant + _dt); + _residual_lpf = (1.f - alpha) * _residual_lpf + alpha * residual; + _signed_innov_test_ratio_lpf = (1.f - alpha) * _signed_innov_test_ratio_lpf + alpha * math::constrain( + signed_innov_test_ratio, -1.f, 1.f); +} + +inline void ZeroOrderHoverThrustEkf::updateMeasurementNoise(const float residual, const float H) +{ + const float alpha = _dt / (_noise_learning_time_constant + _dt); + const float res_no_bias = residual - _residual_lpf; + const float P = _state_var; + _acc_var = math::constrain((1.f - alpha) * _acc_var + alpha * (res_no_bias * res_no_bias + H * P * H), 1.f, 400.f); +} diff --git a/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp new file mode 100644 index 0000000..51672d0 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 zero_order_thrust_ekf.hpp + * @brief Implementation of a single-state hover thrust estimator + * + * state: hover thrust (Th) + * Vertical acceleration is used as a measurement and the current + * thrust (T[k]) is used in the measurement model. + * + * The state is noise driven: Transition matrix A = 1 + * x[k+1] = Ax[k] + v with v ~ N(0, Q) + * y[k] = h(u, x) + w with w ~ N(0, R) + * + * Where the measurement model and corresponding partial derivative (w.r.t. Th) are: + * h(u, x)[k] = g * T[k] / Th[k] - g + * H[k] = -g * T[k] / Th[k]**2 + * + * The measurmement noise variance R is continuously estimated using the residual after each + * measurement update with the following equation: + * R = (1 - alpha) * R + alpha * (z^2 + P * H^2) + * Where z is the residual and alpha a forgetting factor between 0 and 1 + * (see S.Akhlaghi et al., Adaptive adjustment of noise covariance in Kalman filter for dynamic state estimation, 2017) + * + * During the measurment update step, the Normalized Innovation Squared (NIS) is checked + * and the measurement is rejected if larger than the gate size. + * A recovery logic is triggered when too many measurements are continuously rejected and + * consists of a measurement variance reset and a state variance boost. + * + * @author Mathieu Bresciani + */ + +#pragma once + +#include +#include +#include + +class ZeroOrderHoverThrustEkf +{ +public: + ZeroOrderHoverThrustEkf() = default; + ~ZeroOrderHoverThrustEkf() = default; + + void resetAccelNoise() { _acc_var = 5.f; }; + + void predict(float _dt); + void fuseAccZ(float acc_z, float thrust); + + void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); } + void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; } + void setMeasurementNoiseStdDev(float measurement_noise) { _acc_var = measurement_noise * measurement_noise; } + void setHoverThrustStdDev(float hover_thrust_noise) { _state_var = hover_thrust_noise * hover_thrust_noise; } + void setAccelInnovGate(float gate_size) { _gate_size = gate_size; } + + float getHoverThrustEstimate() const { return _hover_thr; } + float getHoverThrustEstimateVar() const { return _state_var; } + float getInnovation() const { return _innov; } + float getInnovationVar() const { return _innov_var; } + float getInnovationTestRatio() const { return _innov_test_ratio; } + float getAccelNoiseVar() const { return _acc_var; } + +private: + float _hover_thr{0.5f}; + + float _gate_size{3.f}; + float _state_var{0.01f}; ///< Initial hover thrust uncertainty variance (thrust^2) + float _process_var{12.5e-6f}; ///< Hover thrust process noise variance (thrust^2/s^2) + float _acc_var{5.f}; ///< Acceleration variance (m^2/s^3) + float _dt{0.02f}; + + float _innov{0.f}; ///< Measurement innovation (m/s^2) + float _innov_var{0.f}; ///< Measurement innovation variance (m^2/s^3) + float _innov_test_ratio{0.f}; ///< Noramlized Innovation Squared test ratio + + float _residual_lpf{}; ///< used to remove the constant bias of the residual + float _signed_innov_test_ratio_lpf{}; ///< used as a delay to trigger the recovery logic + + float computeH(float thrust) const; + float computeInnovVar(float H) const; + float computePredictedAccZ(float thrust) const; + float computeInnov(float acc_z, float thrust) const; + float computeKalmanGain(float H, float innov_var) const; + + /* + * Compute the ratio between the Normalized Innovation Squared (NIS) + * and its maximum gate size. Use isTestRatioPassing to know if the + * measurement should be fused or not. + */ + float computeInnovTestRatio(float innov, float innov_var) const; + bool isTestRatioPassing(float innov_test_ratio) const; + + void updateState(float K, float innov); + void updateStateCovariance(float K, float H); + bool isLargeOffsetDetected() const; + + void bumpStateVariance(); + void updateLpf(float residual, float signed_innov_test_ratio); + void updateMeasurementNoise(float residual, float H); + + static constexpr float _noise_learning_time_constant = 2.f; ///< in seconds + static constexpr float _lpf_time_constant = 1.f; ///< in seconds +}; diff --git a/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp new file mode 100644 index 0000000..69e6549 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Test code for the Zero Order Hover Thrust Estimator + * Run this test only using make tests TESTFILTER=zero_order_hover_thrust_ekf + */ + +#include +#include +#include + +#include "zero_order_hover_thrust_ekf.hpp" + +using namespace matrix; + +class ZeroOrderHoverThrustEkfTest : public ::testing::Test +{ +public: + struct Status { + float hover_thrust; + float hover_thrust_var; + float innov; + float innov_var; + float innov_test_ratio; + float accel_noise_var; + }; + + ZeroOrderHoverThrustEkfTest() + { + _random_generator.seed(42); + } + float computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust); + Status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f, + float thr_noise = 0.f); + +private: + ZeroOrderHoverThrustEkf _ekf{}; + static constexpr float _dt = 0.02f; + + std::normal_distribution _standard_normal_distribution; + std::default_random_engine _random_generator; // Pseudo-random generator with constant seed + +protected: + static constexpr float _accel_noise_var_min = 1.f; // Constrained in the implementation +}; + +float ZeroOrderHoverThrustEkfTest::computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust) +{ + return CONSTANTS_ONE_G * thrust / hover_thrust - CONSTANTS_ONE_G; +} + +ZeroOrderHoverThrustEkfTest::Status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust, + float time, + float accel_noise, float thr_noise) +{ + Status status{}; + + for (float t = 0.f; t <= time; t += _dt) { + _ekf.predict(_dt); + float noisy_thrust = thrust + thr_noise * _standard_normal_distribution(_random_generator); + float accel_theory = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true); + float noisy_accel = accel_theory + accel_noise * _standard_normal_distribution(_random_generator); + _ekf.fuseAccZ(noisy_accel, noisy_thrust); + } + + status.hover_thrust = _ekf.getHoverThrustEstimate(); + status.hover_thrust_var = _ekf.getHoverThrustEstimateVar(); + + status.innov = _ekf.getInnovation(); + status.innov_var = _ekf.getInnovationVar(); + status.innov_test_ratio = _ekf.getInnovationTestRatio(); + status.accel_noise_var = _ekf.getAccelNoiseVar(); + + return status; +} + +TEST_F(ZeroOrderHoverThrustEkfTest, testStaticCase) +{ + // GIVEN: a vehicle at hover, (the estimator starting at the true value) + const float thrust = 0.5f; + const float hover_thrust_true = 0.5f; + + // WHEN: we input noiseless data and run the filter + ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, 2.f); + + // THEN: The estimate should not move and its variance decrease quickly + EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-4f); + EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); + EXPECT_NEAR(status.accel_noise_var, _accel_noise_var_min, + 1.f); // The noise learning is slow and takes more time to go to zero +} + +TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergence) +{ + // GIVEN: a vehicle at hover, but the estimator is starting at hover_thrust = 0.5 + const float thrust = 0.72f; + const float hover_thrust_true = 0.72f; + + // WHEN: we input noiseless data and run the filter + ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, 2.f); + + // THEN: the state should converge to the true value and its variance decrease + EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-2f); + EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); + EXPECT_NEAR(status.accel_noise_var, _accel_noise_var_min, + 1.f); // The noise learning is slow and takes more time to go to zero +} + +TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergenceWithNoise) +{ + // GIVEN: a vehicle at hover, the estimator starts with the wrong estimate and the measurements are noisy + const float sigma_noise = 3.f; + const float noise_var = sigma_noise * sigma_noise; + const float thrust = 0.72f; + const float hover_thrust_true = 0.72f; + const float t_sim = 10.f; + + // WHEN: we input noisy accel data and run the filter + ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise); + + // THEN: the estimate should converge and the accel noise variance should be close to the true noise value + EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f); + EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); + EXPECT_NEAR(status.accel_noise_var, noise_var, 0.2f * noise_var); +} + +TEST_F(ZeroOrderHoverThrustEkfTest, testLargeAccelNoiseAndBias) +{ + // GIVEN: a vehicle descending, the estimator starts with the wrong estimate, the measurements are really noisy + const float sigma_noise = 7.f; + const float noise_var = sigma_noise * sigma_noise; + const float thrust = 0.4f; // Below hover thrust + const float hover_thrust_true = 0.72f; + const float t_sim = 15.f; + + // WHEN: we input noisy accel data and run the filter + ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise); + + // THEN: the estimate should converge and the accel noise variance should be close to the true noise value + EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 7e-2); + EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); + EXPECT_NEAR(status.accel_noise_var, noise_var, 0.2f * noise_var); +} + +TEST_F(ZeroOrderHoverThrustEkfTest, testThrustAndAccelNoise) +{ + // GIVEN: a vehicle climbing, the estimator starts with the wrong estimate, the measurements + // and the input thrust are noisy + const float accel_noise = 2.f; + const float accel_var = accel_noise * accel_noise; + const float thr_noise = 0.01f; + const float thrust = 0.72f; // Above hover thrust + const float hover_thrust_true = 0.6f; + const float t_sim = 15.f; + + // WHEN: we input noisy accel and thrust data, and run the filter + ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise); + + // THEN: the estimate should converge and the accel noise variance should be close to the true noise value + EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f); + EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); + // Because of the nonlinear measurment model and the thust noise, the accel noise estimation is a bit worse + EXPECT_NEAR(status.accel_noise_var, accel_var, 0.4f * accel_var); +} + +TEST_F(ZeroOrderHoverThrustEkfTest, testHoverThrustJump) +{ + // GIVEN: a vehicle hovering, the estimator starts with the wrong estimate, the measurements + // and the input thrust are noisy + const float accel_noise = 2.f; + const float accel_var = accel_noise * accel_noise; + const float thr_noise = 0.01f; + float thrust = 0.8; // At hover + float hover_thrust_true = 0.8f; + float t_sim = 10.f; + + // WHEN: we input noisy accel and thrust data, and run the filter + ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise); + // THEN: change the hover thrust and the current thrust (the velocity controller responds quickly) + // Note that this is an extreme jump in hover thrust + thrust = 0.3; + hover_thrust_true = 0.3f; + t_sim = 10.f; + status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise); + + // THEN: the estimate should converge to the new hover thrust and the accel noise variance should + // be close to the true noise value + EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f); + EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); + // After a recovery, the noise variance estimate takes more time to converge back to the true value + EXPECT_NEAR(status.accel_noise_var, accel_var, 2.f * accel_var); +} diff --git a/src/prometheus_px4/src/modules/mc_pos_control/CMakeLists.txt b/src/prometheus_px4/src/modules/mc_pos_control/CMakeLists.txt new file mode 100644 index 0000000..1b26f79 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(PositionControl) +add_subdirectory(Takeoff) + +px4_add_module( + MODULE modules__mc_pos_control + MAIN mc_pos_control + COMPILE_FLAGS + SRCS + MulticopterPositionControl.cpp + MulticopterPositionControl.hpp + DEPENDS + PositionControl + Takeoff + controllib + git_ecl + ecl_geo + SlewRate + ) diff --git a/src/prometheus_px4/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/prometheus_px4/src/modules/mc_pos_control/MulticopterPositionControl.cpp new file mode 100644 index 0000000..146de65 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -0,0 +1,610 @@ +/**************************************************************************** + * + * 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 "MulticopterPositionControl.hpp" + +#include +#include +#include +#include "PositionControl/ControlMath.hpp" + +using namespace matrix; + +// 无人机多旋翼位置控制模块：实现位置、速度、加速度级联PID控制，是机载操作系统中核心的飞行控制逻辑 +MulticopterPositionControl::MulticopterPositionControl(bool vtol) : + SuperBlock(nullptr, "MPC"), + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD") +{ + parameters_update(true); + _failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND); + _tilt_limit_slew_rate.setSlewRate(.2f); + reset_setpoint_to_nan(_setpoint); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ + perf_free(_cycle_perf); +} + +bool MulticopterPositionControl::init() +{ + if (!_local_pos_sub.registerCallback()) { + PX4_ERR("vehicle_local_position callback registration failed!"); + return false; + } + + _time_stamp_last_loop = hrt_absolute_time(); + ScheduleNow(); + + return true; +} + +int MulticopterPositionControl::parameters_update(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 + ModuleParams::updateParams(); + SuperBlock::updateParams(); + + int num_changed = 0; + + if (_param_sys_vehicle_resp.get() >= 0.f) { + // make it less sensitive at the lower end + float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get(); + + num_changed += _param_mpc_acc_hor.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_hor_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness)); + num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness)); + + if (responsiveness > 0.6f) { + num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); + + } else { + num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); + } + + if (responsiveness < 0.5f) { + num_changed += _param_mpc_tiltmax_air.commit_no_notification(45.f); + + } else { + num_changed += _param_mpc_tiltmax_air.commit_no_notification(math::min(MAX_SAFE_TILT_DEG, math::lerp(45.f, 70.f, + (responsiveness - 0.5f) * 2.f))); + } + + num_changed += _param_mpc_acc_down_max.commit_no_notification(math::lerp(0.8f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_up_max.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); + num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); + } + + if (_param_mpc_xy_vel_all.get() >= 0.f) { + float xy_vel = _param_mpc_xy_vel_all.get(); + num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel); + num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel); + num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel); + } + + if (_param_mpc_z_vel_all.get() >= 0.f) { + float z_vel = _param_mpc_z_vel_all.get(); + num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel); + num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f); + num_changed += _param_mpc_tko_speed.commit_no_notification(z_vel * 0.6f); + num_changed += _param_mpc_land_speed.commit_no_notification(z_vel * 0.5f); + } + + if (num_changed > 0) { + param_notify_changes(); + } + + if (_param_mpc_tiltmax_air.get() > MAX_SAFE_TILT_DEG) { + _param_mpc_tiltmax_air.set(MAX_SAFE_TILT_DEG); + _param_mpc_tiltmax_air.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Tilt constrained to safe value"); + } + + if (_param_mpc_tiltmax_lnd.get() > _param_mpc_tiltmax_air.get()) { + _param_mpc_tiltmax_lnd.set(_param_mpc_tiltmax_air.get()); + _param_mpc_tiltmax_lnd.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Land tilt has been constrained by max tilt"); + } + + _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); + _control.setVelocityGains( + Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()), + Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), + Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); + + // Check that the design parameters are inside the absolute maximum constraints + if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { + _param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get()); + _param_mpc_xy_cruise.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed"); + } + + if (_param_mpc_vel_manual.get() > _param_mpc_xy_vel_max.get()) { + _param_mpc_vel_manual.set(_param_mpc_xy_vel_max.get()); + _param_mpc_vel_manual.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed"); + } + + if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() || + _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) { + _param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(), + _param_mpc_thr_max.get())); + _param_mpc_thr_hover.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max"); + } + + if (!_param_mpc_use_hte.get() || !_hover_thrust_initialized) { + _control.setHoverThrust(_param_mpc_thr_hover.get()); + _hover_thrust_initialized = true; + } + + // initialize vectors from params and enforce constraints + _param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get())); + _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get())); + + _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); + _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); + _takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get()); + } + + return OK; +} + +PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s &local_pos) +{ + PositionControlStates states; + + // only set position states if valid and finite + if (PX4_ISFINITE(local_pos.x) && PX4_ISFINITE(local_pos.y) && local_pos.xy_valid) { + states.position(0) = local_pos.x; + states.position(1) = local_pos.y; + + } else { + states.position(0) = NAN; + states.position(1) = NAN; + } + + if (PX4_ISFINITE(local_pos.z) && local_pos.z_valid) { + states.position(2) = local_pos.z; + + } else { + states.position(2) = NAN; + } + + if (PX4_ISFINITE(local_pos.vx) && PX4_ISFINITE(local_pos.vy) && local_pos.v_xy_valid) { + states.velocity(0) = local_pos.vx; + states.velocity(1) = local_pos.vy; + states.acceleration(0) = _vel_x_deriv.update(local_pos.vx); + states.acceleration(1) = _vel_y_deriv.update(local_pos.vy); + + } else { + states.velocity(0) = NAN; + states.velocity(1) = NAN; + states.acceleration(0) = NAN; + states.acceleration(1) = NAN; + + // reset derivatives to prevent acceleration spikes when regaining velocity + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } + + if (PX4_ISFINITE(local_pos.vz) && local_pos.v_z_valid) { + states.velocity(2) = local_pos.vz; + states.acceleration(2) = _vel_z_deriv.update(states.velocity(2)); + + } else { + states.velocity(2) = NAN; + states.acceleration(2) = NAN; + + // reset derivative to prevent acceleration spikes when regaining velocity + _vel_z_deriv.reset(); + } + + states.yaw = local_pos.heading; + + return states; +} + +void MulticopterPositionControl::Run() +{ + if (should_exit()) { + _local_pos_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + // reschedule backup + ScheduleDelayed(100_ms); + + parameters_update(false); + + perf_begin(_cycle_perf); + vehicle_local_position_s local_pos; + + if (_local_pos_sub.update(&local_pos)) { + const hrt_abstime time_stamp_now = local_pos.timestamp; + const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); + _time_stamp_last_loop = time_stamp_now; + + // set _dt in controllib Block for BlockDerivative + setDt(dt); + + const bool was_in_failsafe = _in_failsafe; + _in_failsafe = false; + + _control_mode_sub.update(&_control_mode); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + + if (_param_mpc_use_hte.get()) { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _control.updateHoverThrust(hte.hover_thrust); + } + } + } + + PositionControlStates states{set_vehicle_states(local_pos)}; + + if (_control_mode.flag_multicopter_position_control_enabled) { + + _trajectory_setpoint_sub.update(&_setpoint); + + // adjust existing (or older) setpoint with any EKF reset deltas + if (_setpoint.timestamp < local_pos.timestamp) { + if (local_pos.vxy_reset_counter != _vxy_reset_counter) { + _setpoint.vx += local_pos.delta_vxy[0]; + _setpoint.vy += local_pos.delta_vxy[1]; + } + + if (local_pos.vz_reset_counter != _vz_reset_counter) { + _setpoint.vz += local_pos.delta_vz; + } + + if (local_pos.xy_reset_counter != _xy_reset_counter) { + _setpoint.x += local_pos.delta_xy[0]; + _setpoint.y += local_pos.delta_xy[1]; + } + + if (local_pos.z_reset_counter != _z_reset_counter) { + _setpoint.z += local_pos.delta_z; + } + + if (local_pos.heading_reset_counter != _heading_reset_counter) { + _setpoint.yaw += local_pos.delta_heading; + } + } + + // update vehicle constraints and handle smooth takeoff + _vehicle_constraints_sub.update(&_vehicle_constraints); + + // fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN + // TODO: this should get obsolete once the takeoff limiting moves into the flight tasks + if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) { + _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); + } + + if (_control_mode.flag_control_offboard_enabled) { + + bool want_takeoff = _control_mode.flag_armed && _vehicle_land_detected.landed + && hrt_elapsed_time(&_setpoint.timestamp) < 1_s; + + if (want_takeoff && PX4_ISFINITE(_setpoint.z) + && (_setpoint.z < states.position(2))) { + + _vehicle_constraints.want_takeoff = true; + + } else if (want_takeoff && PX4_ISFINITE(_setpoint.vz) + && (_setpoint.vz < 0.f)) { + + _vehicle_constraints.want_takeoff = true; + + } else if (want_takeoff && PX4_ISFINITE(_setpoint.acceleration[2]) + && (_setpoint.acceleration[2] < 0.f)) { + + _vehicle_constraints.want_takeoff = true; + + } else { + _vehicle_constraints.want_takeoff = false; + } + + // override with defaults + _vehicle_constraints.speed_xy = _param_mpc_xy_vel_max.get(); + _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); + _vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + } + + // handle smooth takeoff + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff, + _vehicle_constraints.speed_up, false, time_stamp_now); + + const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup); + const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); + const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact); + + // make sure takeoff ramp is not amended by acceleration feed-forward + if (!flying) { + _setpoint.acceleration[2] = NAN; + } + + if (not_taken_off || flying_but_ground_contact) { + // we are not flying yet and need to avoid any corrections + reset_setpoint_to_nan(_setpoint); + Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust + + // prevent any integrator windup + _control.resetIntegral(); + } + + // limit tilt during takeoff ramupup + const float tilt_limit_deg = (_takeoff.getTakeoffState() < TakeoffState::flight) + ? _param_mpc_tiltmax_lnd.get() : _param_mpc_tiltmax_air.get(); + _control.setTiltLimit(_tilt_limit_slew_rate.update(math::radians(tilt_limit_deg), dt)); + + const float speed_up = _takeoff.updateRamp(dt, + PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get()); + const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down : + _param_mpc_z_vel_max_dn.get(); + const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy : + _param_mpc_xy_vel_max.get(); + + // Allow ramping from zero thrust on takeoff + const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f; + + _control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get()); + + _control.setVelocityLimits( + math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()), + math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limit + math::constrain(speed_down, 0.f, _param_mpc_z_vel_max_dn.get())); + + _control.setInputSetpoint(_setpoint); + + // update states + if (!PX4_ISFINITE(_setpoint.z) + && PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON) + && PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) { + // A change in velocity is demanded and the altitude is not controlled. + // Set velocity to the derivative of position + // because it has less bias but blend it in across the landing speed range + // < MPC_LAND_SPEED: ramp up using altitude derivative without a step + // >= MPC_LAND_SPEED: use altitude derivative + float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f); + states.velocity(2) = local_pos.z_deriv * weighting + local_pos.vz * (1.f - weighting); + } + + _control.setState(states); + + // Run position control + if (_control.update(dt)) { + _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now); + + } else { + // Failsafe + if ((time_stamp_now - _last_warn) > 2_s && _last_warn > 0) { + PX4_WARN("invalid setpoints"); + _last_warn = time_stamp_now; + } + + vehicle_local_position_setpoint_s failsafe_setpoint{}; + + failsafe(time_stamp_now, failsafe_setpoint, states, !was_in_failsafe); + + // reset constraints + _vehicle_constraints = {0, NAN, NAN, NAN, false, {}}; + + _control.setInputSetpoint(failsafe_setpoint); + _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); + _control.update(dt); + } + + // Publish internal position control setpoints + // on top of the input/feed-forward setpoints these containt the PID corrections + // This message is used by other modules (such as Landdetector) to determine vehicle intention. + vehicle_local_position_setpoint_s local_pos_sp{}; + _control.getLocalPositionSetpoint(local_pos_sp); + local_pos_sp.timestamp = hrt_absolute_time(); + _local_pos_sp_pub.publish(local_pos_sp); + + // Publish attitude setpoint output + vehicle_attitude_setpoint_s attitude_setpoint{}; + _control.getAttitudeSetpoint(attitude_setpoint); + attitude_setpoint.timestamp = hrt_absolute_time(); + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + } else { + // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, time_stamp_now); + } + + // Publish takeoff status + const uint8_t takeoff_state = static_cast(_takeoff.getTakeoffState()); + + if (takeoff_state != _takeoff_status_pub.get().takeoff_state + || !isEqualF(_tilt_limit_slew_rate.getState(), _takeoff_status_pub.get().tilt_limit)) { + _takeoff_status_pub.get().takeoff_state = takeoff_state; + _takeoff_status_pub.get().tilt_limit = _tilt_limit_slew_rate.getState(); + _takeoff_status_pub.get().timestamp = hrt_absolute_time(); + _takeoff_status_pub.update(); + } + + // save latest reset counters + _vxy_reset_counter = local_pos.vxy_reset_counter; + _vz_reset_counter = local_pos.vz_reset_counter; + _xy_reset_counter = local_pos.xy_reset_counter; + _z_reset_counter = local_pos.z_reset_counter; + _heading_reset_counter = local_pos.heading_reset_counter; + } + + perf_end(_cycle_perf); +} + +void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, + const PositionControlStates &states, bool warn) +{ + // do not warn while we are disarmed, as we might not have valid setpoints yet + if (!_control_mode.flag_armed) { + warn = false; + } + + // Only react after a short delay + _failsafe_land_hysteresis.set_state_and_update(true, now); + + if (_failsafe_land_hysteresis.get_state()) { + reset_setpoint_to_nan(setpoint); + + if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) { + // don't move along xy + setpoint.vx = setpoint.vy = 0.f; + + if (warn) { + PX4_WARN("Failsafe: stop and wait"); + } + + } else { + // descend with land speed since we can't stop + setpoint.acceleration[0] = setpoint.acceleration[1] = 0.f; + setpoint.vz = _param_mpc_land_speed.get(); + + if (warn) { + PX4_WARN("Failsafe: blind land"); + } + } + + if (PX4_ISFINITE(states.velocity(2))) { + // don't move along z if we can stop in all dimensions + if (!PX4_ISFINITE(setpoint.vz)) { + setpoint.vz = 0.f; + } + + } else { + // emergency descend with a bit below hover thrust + setpoint.vz = NAN; + setpoint.acceleration[2] = .3f; + + if (warn) { + PX4_WARN("Failsafe: blind descend"); + } + } + + _in_failsafe = true; + } +} + +void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint) +{ + setpoint.x = setpoint.y = setpoint.z = NAN; + setpoint.vx = setpoint.vy = setpoint.vz = NAN; + setpoint.yaw = setpoint.yawspeed = NAN; + setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN; + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; +} + +int MulticopterPositionControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + MulticopterPositionControl *instance = new MulticopterPositionControl(vtol); + + 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 MulticopterPositionControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterPositionControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The controller has two loops: a P loop for position error and a PID loop for velocity error. +Output of the velocity controller is thrust vector that is split to thrust direction +(i.e. rotation matrix for multicopter orientation) and thrust scalar (i.e. multicopter thrust itself). + +The controller doesn't use Euler angles for its work, they are generated only for more human-friendly control and +logging. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_pos_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]) +{ + return MulticopterPositionControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/prometheus_px4/src/modules/mc_pos_control/MulticopterPositionControl.hpp new file mode 100644 index 0000000..1e6b591 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -0,0 +1,243 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Multicopter position controller. + */ + +#pragma once + +#include "PositionControl/PositionControl.hpp" +#include "Takeoff/Takeoff.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, + public ModuleParams, public px4::ScheduledWorkItem +{ +public: + MulticopterPositionControl(bool vtol = false); + ~MulticopterPositionControl() 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; + + Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ + + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::PublicationData _takeoff_status_pub{ORB_ID(takeoff_status)}; + uORB::Publication _vehicle_attitude_setpoint_pub; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)}; + + hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + + int _task_failure_count{0}; /**< counter for task failures */ + + vehicle_control_mode_s _control_mode{}; + vehicle_local_position_setpoint_s _setpoint{}; + vehicle_constraints_s _vehicle_constraints{ + .timestamp = 0, + .speed_xy = NAN, + .speed_up = NAN, + .speed_down = NAN, + .want_takeoff = false, + }; + + vehicle_land_detected_s _vehicle_land_detected { + .timestamp = 0, + .alt_max = -1.0f, + .freefall = false, + .ground_contact = true, + .maybe_landed = true, + .landed = true, + }; + + DEFINE_PARAMETERS( + // Position Control + (ParamFloat) _param_mpc_xy_p, + (ParamFloat) _param_mpc_z_p, + (ParamFloat) _param_mpc_xy_vel_p_acc, + (ParamFloat) _param_mpc_xy_vel_i_acc, + (ParamFloat) _param_mpc_xy_vel_d_acc, + (ParamFloat) _param_mpc_z_vel_p_acc, + (ParamFloat) _param_mpc_z_vel_i_acc, + (ParamFloat) _param_mpc_z_vel_d_acc, + (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_mpc_z_vel_max_dn, + (ParamFloat) _param_mpc_tiltmax_air, + (ParamFloat) _param_mpc_thr_hover, + (ParamBool) _param_mpc_use_hte, + + // Takeoff / Land + (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ + (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ + (ParamFloat) _param_mpc_tko_speed, + (ParamFloat) _param_mpc_land_speed, + + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_xy_cruise, + (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ + (ParamInt) _param_mpc_pos_mode, + (ParamInt) _param_mpc_alt_mode, + (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ + (ParamFloat) _param_mpc_thr_min, + (ParamFloat) _param_mpc_thr_max, + + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_hor_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + + (ParamFloat) _param_mpc_xy_vel_all, + (ParamFloat) _param_mpc_z_vel_all + ); + + control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ + control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ + control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ + + PositionControl _control; /**< class for core PID position control */ + + hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ + + bool _in_failsafe{false}; /**< true if failsafe was entered within current cycle */ + + bool _hover_thrust_initialized{false}; + + /** Timeout in us for trajectory data to get considered invalid */ + static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; + /** If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land */ + static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms; + /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off and tilt is limited */ + static constexpr float ALTITUDE_THRESHOLD = 0.3f; + + static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf + + systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ + SlewRate _tilt_limit_slew_rate; + + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _xy_reset_counter{0}; + uint8_t _z_reset_counter{0}; + uint8_t _heading_reset_counter{0}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + + /** + * Update our local parameter cache. + * Parameter update can be forced when argument is true. + * @param force forces parameter update. + */ + int parameters_update(bool force); + + /** + * Check for validity of positon/velocity states. + */ + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos); + + /** + * Adjust the setpoint during landing. + * Thrust is adjusted to support the land-detector during detection. + * @param setpoint gets adjusted based on land-detector state + */ + void limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint); + + /** + * Failsafe. + * If flighttask fails for whatever reason, then do failsafe. This could + * occur if the commander fails to switch to a mode in case of invalid states or + * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set + * to true, the failsafe will be initiated immediately. + */ + void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, + bool warn); + + /** + * Reset setpoints to NAN + */ + void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); +}; diff --git a/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/CMakeLists.txt b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/CMakeLists.txt new file mode 100644 index 0000000..8136317 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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(PositionControl + ControlMath.cpp + ControlMath.hpp + PositionControl.cpp + PositionControl.hpp +) +target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl) +px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl) diff --git a/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMath.cpp new file mode 100644 index 0000000..c540bac --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * Copyright (C) 2018 - 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 ControlMath.cpp + */ + +#include "ControlMath.hpp" +#include +#include +#include + +using namespace matrix; + +namespace ControlMath +{ +void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + bodyzToAttitude(-thr_sp, yaw_sp, att_sp); + att_sp.thrust_body[2] = -thr_sp.length(); +} + +void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle) +{ + // determine tilt + const float dot_product_unit = body_unit.dot(world_unit); + float angle = acosf(dot_product_unit); + // limit tilt + angle = math::min(angle, max_angle); + Vector3f rejection = body_unit - (dot_product_unit * world_unit); + + // corner case exactly parallel vectors + if (rejection.norm_squared() < FLT_EPSILON) { + rejection(0) = 1.f; + } + + body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit(); +} + +void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + // zero vector, no direction, set safe level value + if (body_z.norm_squared() < FLT_EPSILON) { + body_z(2) = 1.f; + } + + body_z.normalize(); + + // vector of desired yaw direction in XY plane, rotated by PI/2 + const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f}; + + // desired body_x axis, orthogonal to body_z + Vector3f body_x = y_C % body_z; + + // keep nose to front while inverted upside down + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + if (fabsf(body_z(2)) < 0.000001f) { + // desired thrust is in XY plane, set X downside to construct correct matrix, + // but yaw component will not be used actually + body_x.zero(); + body_x(2) = 1.0f; + } + + body_x.normalize(); + + // desired body_y axis + const Vector3f body_y = body_z % body_x; + + Dcmf R_sp; + + // fill rotation matrix + for (int i = 0; i < 3; i++) { + R_sp(i, 0) = body_x(i); + R_sp(i, 1) = body_y(i); + R_sp(i, 2) = body_z(i); + } + + // copy quaternion setpoint to attitude setpoint topic + const Quatf q_sp{R_sp}; + q_sp.copyTo(att_sp.q_d); + + // calculate euler angles, for logging only, must not be used for control + const Eulerf euler{R_sp}; + att_sp.roll_body = euler.phi(); + att_sp.pitch_body = euler.theta(); + att_sp.yaw_body = euler.psi(); +} + +Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) +{ + if (Vector2f(v0 + v1).norm() <= max) { + // vector does not exceed maximum magnitude + return v0 + v1; + + } else if (v0.length() >= max) { + // the magnitude along v0, which has priority, already exceeds maximum. + return v0.normalized() * max; + + } else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) { + // the two vectors are equal + return v0.normalized() * max; + + } else if (v0.length() < 0.001f) { + // the first vector is 0. + return v1.normalized() * max; + + } else { + // vf = final vector with ||vf|| <= max + // s = scaling factor + // u1 = unit of v1 + // vf = v0 + v1 = v0 + s * u1 + // constraint: ||vf|| <= max + // + // solve for s: ||vf|| = ||v0 + s * u1|| <= max + // + // Derivation: + // For simplicity, replace v0 -> v, u1 -> u + // v0(0/1/2) -> v0/1/2 + // u1(0/1/2) -> u0/1/2 + // + // ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2 + // v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2 + // s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0 + // + // quadratic equation: + // -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a) + // + // b = 2 * u.dot(v) + // a = 1 (because u is normalized) + // c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2 + // + // sqrt(b^2 - 4*a*c) = + // sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2)) + // + // s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2 + // = -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2)) + // m = u.dot(v) + // s = -m + sqrt(m^2 - c) + // + // + // + // notes: + // - s (=scaling factor) needs to be positive + // - (max - ||v||) always larger than zero, otherwise it never entered this if-statement + Vector2f u1 = v1.normalized(); + float m = u1.dot(v0); + float c = v0.dot(v0) - max * max; + float s = -m + sqrtf(m * m - c); + return v0 + u1 * s; + } +} + +bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, + const Vector3f &line_a, const Vector3f &line_b, Vector3f &res) +{ + // project center of sphere on line normalized AB + Vector3f ab_norm = line_b - line_a; + + if (ab_norm.length() < 0.01f) { + return true; + } + + ab_norm.normalize(); + Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + if (sphere_r > cd_len) { + // we have triangle CDX with known CD and CX = R, find DX + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + + if ((sphere_c - line_b) * ab_norm > 0.0f) { + // target waypoint is already behind us + res = line_b; + + } else { + // target is in front of us + res = d + ab_norm * dx_len; // vector A->B on line + } + + return true; + + } else { + + // have no roots, return D + res = d; // go directly to line + + // previous waypoint is still in front of us + if ((sphere_c - line_a) * ab_norm < 0.0f) { + res = line_a; + } + + // target waypoint is already behind us + if ((sphere_c - line_b) * ab_norm > 0.0f) { + res = line_b; + } + + return false; + } +} + +void addIfNotNan(float &setpoint, const float addition) +{ + if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) { + // No NAN, add to the setpoint + setpoint += addition; + + } else if (!PX4_ISFINITE(setpoint)) { + // Setpoint NAN, take addition + setpoint = addition; + } + + // Addition is NAN or both are NAN, nothing to do +} + +void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition) +{ + for (int i = 0; i < 3; i++) { + addIfNotNan(setpoint(i), addition(i)); + } +} + +void setZeroIfNanVector3f(Vector3f &vector) +{ + // Adding zero vector overwrites elements that are NaN with zero + addIfNotNanVector3f(vector, Vector3f()); +} + +} // ControlMath diff --git a/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMath.hpp new file mode 100644 index 0000000..e356d94 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2018 - 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 ControlMath.hpp + * + * Simple functions for vector manipulation that do not fit into matrix lib. + * These functions are specific for controls. + */ + +#pragma once + +#include +#include + +namespace ControlMath +{ +/** + * Converts thrust vector and yaw set-point to a desired attitude. + * @param thr_sp desired 3D thrust vector + * @param yaw_sp the desired yaw + * @param att_sp attitude setpoint to fill + */ +void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Limits the tilt angle between two unit vectors + * @param body_unit unit vector that will get adjusted if angle is too big + * @param world_unit fixed vector to measure the angle against + * @param max_angle maximum tilt angle between vectors in radians + */ +void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit, const float max_angle); + +/** + * Converts a body z vector and yaw set-point to a desired attitude. + * @param body_z a world frame 3D vector in direction of the desired body z axis + * @param yaw_sp the desired yaw setpoint + * @param att_sp attitude setpoint to fill + */ +void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Outputs the sum of two vectors but respecting the limits and priority. + * The sum of two vectors are constraint such that v0 has priority over v1. + * This means that if the length of (v0+v1) exceeds max, then it is constraint such + * that v0 has priority. + * + * @param v0 a 2D vector that has priority given the maximum available magnitude. + * @param v1 a 2D vector that less priority given the maximum available magnitude. + * @return 2D vector + */ +matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float &max); + +/** + * This method was used for smoothing the corners along two lines. + * + * @param sphere_c + * @param sphere_r + * @param line_a + * @param line_b + * @param res + * return boolean + * + * Note: this method is not used anywhere and first requires review before usage. + */ +bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a, + const matrix::Vector3f &line_b, matrix::Vector3f &res); + +/** + * Adds e.g. feed-forward to the setpoint making sure existing or added NANs have no influence on control. + * This function is udeful to support all the different setpoint combinations of position, velocity, acceleration with NAN representing an uncommited value. + * @param setpoint existing possibly NAN setpoint to add to + * @param addition value/NAN to add to the setpoint + */ +void addIfNotNan(float &setpoint, const float addition); + +/** + * _addIfNotNan for Vector3f treating each element individually + * @see _addIfNotNan + */ +void addIfNotNanVector3f(matrix::Vector3f &setpoint, const matrix::Vector3f &addition); + +/** + * Overwrites elements of a Vector3f which are NaN with zero + * @param vector possibly containing NAN elements + */ +void setZeroIfNanVector3f(matrix::Vector3f &vector); +} diff --git a/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp new file mode 100644 index 0000000..0b95451 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * 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 +#include +#include + +using namespace matrix; +using namespace ControlMath; + +TEST(ControlMathTest, LimitTiltUnchanged) +{ + Vector3f body = Vector3f(0.f, 0.f, 1.f).normalized(); + Vector3f body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); + + body = Vector3f(0.f, .1f, 1.f).normalized(); + body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); +} + +TEST(ControlMathTest, LimitTiltOpposite) +{ + Vector3f body = Vector3f(0.f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTiltAlmostOpposite) +{ + // This case doesn't trigger corner case handling but is very close to it + Vector3f body = Vector3f(0.001f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTilt45degree) +{ + Vector3f body = Vector3f(1.f, 0.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(M_SQRT1_2_F, 0, M_SQRT1_2_F)); + + body = Vector3f(0.f, 1.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(0.f, M_SQRT1_2_F, M_SQRT1_2_F)); +} + +TEST(ControlMathTest, LimitTilt10degree) +{ + Vector3f body = Vector3f(1.f, 1.f, .1f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(body(0), body(1)); + + body = Vector3f(1, 2, .2f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(2.f * body(0), body(1)); +} + +TEST(ControlMathTest, ThrottleAttitudeMapping) +{ + /* expected: zero roll, zero pitch, zero yaw, full thr mag + * reason: thrust pointing full upward */ + Vector3f thr{0.f, 0.f, -1.f}; + float yaw = 0.f; + vehicle_attitude_setpoint_s att{}; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, 0.f); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but with 90 yaw + * reason: only yaw changed */ + yaw = M_PI_2_F; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but roll 180 + * reason: thrust points straight down and order Euler + * order is: 1. roll, 2. pitch, 3. yaw */ + thr = Vector3f(0.f, 0.f, 1.f); + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); +} + +TEST(ControlMathTest, ConstrainXYPriorities) +{ + const float max = 5.f; + // v0 already at max + Vector2f v0(max, 0.f); + Vector2f v1(v0(1), -v0(0)); + + Vector2f v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), max); + EXPECT_FLOAT_EQ(v_r(1), 0.f); + + // norm of v1 exceeds max but v0 is zero + v0.zero(); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(1), -max); + EXPECT_FLOAT_EQ(v_r(0), 0.f); + + v0 = Vector2f(.5f, .5f); + v1 = Vector2f(.5f, -.5f); + v_r = constrainXY(v0, v1, max); + const float diff = Vector2f(v_r - (v0 + v1)).length(); + EXPECT_FLOAT_EQ(diff, 0.f); + + // v0 and v1 exceed max and are perpendicular + v0 = Vector2f(4.f, 0.f); + v1 = Vector2f(0.f, -4.f); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), v0(0)); + EXPECT_GT(v_r(0), 0.f); + const float remaining = sqrtf(max * max - (v0(0) * v0(0))); + EXPECT_FLOAT_EQ(v_r(1), -remaining); +} + +TEST(ControlMathTest, CrossSphereLine) +{ + /* Testing 9 positions (+) around waypoints (o): + * + * Far + + + + * + * Near + + + + * On trajectory --+----o---------+---------o----+-- + * prev curr + * + * Expected targets (1, 2, 3): + * Far + + + + * + * + * On trajectory -------1---------2---------3------- + * + * + * Near + + + + * On trajectory -------o---1---------2-----3------- + * + * + * On trajectory --+----o----1----+--------2/3---+-- */ + const Vector3f prev = Vector3f(0.f, 0.f, 0.f); + const Vector3f curr = Vector3f(0.f, 0.f, 2.f); + Vector3f res; + bool retval = false; + + // on line, near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.5f)); + + // on line, near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // on line, near, after target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, .366025388f)); + + // near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.866025448f)); + + // near, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, .5f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // far, before previous waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, -.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f()); + + // far, before target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 1.f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.f)); + + // far, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 2.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); +} + +TEST(ControlMathTest, addIfNotNan) +{ + float v = 1.f; + // regular addition + ControlMath::addIfNotNan(v, 2.f); + EXPECT_EQ(v, 3.f); + // addition is NAN and has no influence + ControlMath::addIfNotNan(v, NAN); + EXPECT_EQ(v, 3.f); + v = NAN; + // both summands are NAN + ControlMath::addIfNotNan(v, NAN); + EXPECT_TRUE(isnan(v)); + // regular value gets added to NAN and overwrites it + ControlMath::addIfNotNan(v, 3.f); + EXPECT_EQ(v, 3.f); +} diff --git a/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControl.cpp new file mode 100644 index 0000000..caca1c8 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 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 PositionControl.cpp + */ + +#include "PositionControl.hpp" +#include "ControlMath.hpp" +#include +#include +#include +#include + +using namespace matrix; + +void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_vel_p = P; + _gain_vel_i = I; + _gain_vel_d = D; +} + +void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down) +{ + _lim_vel_horizontal = vel_horizontal; + _lim_vel_up = vel_up; + _lim_vel_down = vel_down; +} + +void PositionControl::setThrustLimits(const float min, const float max) +{ + // make sure there's always enough thrust vector length to infer the attitude + _lim_thr_min = math::max(min, 10e-4f); + _lim_thr_max = max; +} + +void PositionControl::updateHoverThrust(const float hover_thrust_new) +{ + _vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new); + setHoverThrust(hover_thrust_new); +} + +void PositionControl::setState(const PositionControlStates &states) +{ + _pos = states.position; + _vel = states.velocity; + _yaw = states.yaw; + _vel_dot = states.acceleration; +} + +void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint) +{ + _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); + _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); + _acc_sp = Vector3f(setpoint.acceleration); + _yaw_sp = setpoint.yaw; + _yawspeed_sp = setpoint.yawspeed; +} + +bool PositionControl::update(const float dt) +{ + // x and y input setpoints always have to come in pairs + const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1))) + && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1))) + && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1))); + + _positionControl(); + _velocityControl(dt); + + _yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f; + _yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control + + return valid && _updateSuccessful(); +} + +void PositionControl::_positionControl() +{ + // P-position controller + Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); + // Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence + ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position); + // make sure there are no NAN elements for further reference while constraining + ControlMath::setZeroIfNanVector3f(vel_sp_position); + + // Constrain horizontal velocity by prioritizing the velocity component along the + // the desired position setpoint over the feed-forward term. + _vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal); + // Constrain velocity in z-direction. + _vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down); +} + +void PositionControl::_velocityControl(const float dt) +{ + // PID velocity control + Vector3f vel_error = _vel_sp - _vel; + Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); + + // No control input from setpoints or corresponding states which are NAN + ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); + + _accelerationControl(); + + // Integrator anti-windup in vertical direction + if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) || + (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) { + vel_error(2) = 0.f; + } + + // Saturate maximal vertical thrust + _thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max); + + // Get allowed horizontal thrust after prioritizing vertical control + const float thrust_max_squared = _lim_thr_max * _lim_thr_max; + const float thrust_z_squared = _thr_sp(2) * _thr_sp(2); + const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared; + float thrust_max_xy = 0; + + if (thrust_max_xy_squared > 0) { + thrust_max_xy = sqrtf(thrust_max_xy_squared); + } + + // Saturate thrust in horizontal direction + const Vector2f thrust_sp_xy(_thr_sp); + const float thrust_sp_xy_norm = thrust_sp_xy.norm(); + + if (thrust_sp_xy_norm > thrust_max_xy) { + _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy; + } + + // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output + // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 + const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust); + const float arw_gain = 2.f / _gain_vel_p(0); + vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited)); + + // Make sure integral doesn't get NAN + ControlMath::setZeroIfNanVector3f(vel_error); + // Update integral part of velocity control + _vel_int += vel_error.emult(_gain_vel_i) * dt; + + // limit thrust integral + _vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2)); +} + +void PositionControl::_accelerationControl() +{ + // Assume standard acceleration due to gravity in vertical direction for attitude generation + Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); + ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); + // Scale thrust assuming hover thrust produces standard gravity + float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; + // Project thrust to planned body attitude + collective_thrust /= (Vector3f(0, 0, 1).dot(body_z)); + collective_thrust = math::min(collective_thrust, -_lim_thr_min); + _thr_sp = body_z * collective_thrust; +} + +bool PositionControl::_updateSuccessful() +{ + bool valid = true; + + // For each controlled state the estimate has to be valid + for (int i = 0; i <= 2; i++) { + if (PX4_ISFINITE(_pos_sp(i))) { + valid = valid && PX4_ISFINITE(_pos(i)); + } + + if (PX4_ISFINITE(_vel_sp(i))) { + valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i)); + } + } + + // There has to be a valid output accleration and thrust setpoint otherwise there was no + // setpoint-state pair for each axis that can get controlled + valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2)); + valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2)); + return valid; +} + +void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const +{ + local_position_setpoint.x = _pos_sp(0); + local_position_setpoint.y = _pos_sp(1); + local_position_setpoint.z = _pos_sp(2); + local_position_setpoint.yaw = _yaw_sp; + local_position_setpoint.yawspeed = _yawspeed_sp; + local_position_setpoint.vx = _vel_sp(0); + local_position_setpoint.vy = _vel_sp(1); + local_position_setpoint.vz = _vel_sp(2); + _acc_sp.copyTo(local_position_setpoint.acceleration); + _thr_sp.copyTo(local_position_setpoint.thrust); +} + +void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const +{ + ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); + attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; +} diff --git a/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControl.hpp new file mode 100644 index 0000000..c486499 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 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 PositionControl.hpp + * + * A cascaded position controller for position/velocity control only. + */ + +#pragma once + +#include +#include +#include +#include + +struct PositionControlStates { + matrix::Vector3f position; + matrix::Vector3f velocity; + matrix::Vector3f acceleration; + float yaw; +}; + +/** + * Core Position-Control for MC. + * This class contains P-controller for position and + * PID-controller for velocity. + * Inputs: + * vehicle position/velocity/yaw + * desired set-point position/velocity/thrust/yaw/yaw-speed + * constraints that are stricter than global limits + * Output + * thrust vector and a yaw-setpoint + * + * If there is a position and a velocity set-point present, then + * the velocity set-point is used as feed-forward. If feed-forward is + * active, then the velocity component of the P-controller output has + * priority over the feed-forward component. + * + * A setpoint that is NAN is considered as not set. + * If there is a position/velocity- and thrust-setpoint present, then + * the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop. + */ +class PositionControl +{ +public: + + PositionControl() = default; + ~PositionControl() = default; + + /** + * Set the position control gains + * @param P 3D vector of proportional gains for x,y,z axis + */ + void setPositionGains(const matrix::Vector3f &P) { _gain_pos_p = P; } + + /** + * Set the velocity control gains + * @param P 3D vector of proportional gains for x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + + /** + * Set the maximum velocity to execute with feed forward and position control + * @param vel_horizontal horizontal velocity limit + * @param vel_up upwards velocity limit + * @param vel_down downwards velocity limit + */ + void setVelocityLimits(const float vel_horizontal, const float vel_up, float vel_down); + + /** + * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller + * @param min minimum thrust e.g. 0.1 or 0 + * @param max maximum thrust e.g. 0.9 or 1 + */ + void setThrustLimits(const float min, const float max); + + /** + * Set the maximum tilt angle in radians the output attitude is allowed to have + * @param tilt angle in radians from level orientation + */ + void setTiltLimit(const float tilt) { _lim_tilt = tilt; } + + /** + * Set the normalized hover thrust + * @param thrust [0.1, 0.9] with which the vehicle hovers not acelerating down or up with level orientation + */ + void setHoverThrust(const float hover_thrust) { _hover_thrust = math::constrain(hover_thrust, 0.1f, 0.9f); } + + /** + * Update the hover thrust without immediately affecting the output + * by adjusting the integrator. This prevents propagating the dynamics + * of the hover thrust signal directly to the output of the controller. + */ + void updateHoverThrust(const float hover_thrust_new); + + /** + * Pass the current vehicle state to the controller + * @param PositionControlStates structure + */ + void setState(const PositionControlStates &states); + + /** + * Pass the desired setpoints + * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. + * @param setpoint a vehicle_local_position_setpoint_s structure + */ + void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint); + + /** + * Apply P-position and PID-velocity controller that updates the member + * thrust, yaw- and yawspeed-setpoints. + * @see _thr_sp + * @see _yaw_sp + * @see _yawspeed_sp + * @param dt time in seconds since last iteration + * @return true if update succeeded and output setpoint is executable, false if not + */ + bool update(const float dt); + + /** + * Set the integral term in xy to 0. + * @see _vel_int + */ + void resetIntegral() { _vel_int.setZero(); } + + /** + * Get the controllers output local position setpoint + * These setpoints are the ones which were executed on including PID output and feed-forward. + * The acceleration or thrust setpoints can be used for attitude control. + * @param local_position_setpoint reference to struct to fill up + */ + void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const; + + /** + * Get the controllers output attitude setpoint + * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control. + * It needs to be executed by the attitude controller to achieve velocity and position tracking. + * @param attitude_setpoint reference to struct to fill up + */ + void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; + +private: + bool _updateSuccessful(); + + void _positionControl(); ///< Position proportional control + void _velocityControl(const float dt); ///< Velocity PID control + void _accelerationControl(); ///< Acceleration setpoint processing + + // Gains + matrix::Vector3f _gain_pos_p; ///< Position control proportional gain + matrix::Vector3f _gain_vel_p; ///< Velocity control proportional gain + matrix::Vector3f _gain_vel_i; ///< Velocity control integral gain + matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain + + // Limits + float _lim_vel_horizontal{}; ///< Horizontal velocity limit with feed forward and position control + float _lim_vel_up{}; ///< Upwards velocity limit with feed forward and position control + float _lim_vel_down{}; ///< Downwards velocity limit with feed forward and position control + float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9 + float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 + float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have + + float _hover_thrust{}; ///< Thrust [0.1, 0.9] with which the vehicle hovers not accelerating down or up with level orientation + + // States + matrix::Vector3f _pos; /**< current position */ + matrix::Vector3f _vel; /**< current velocity */ + matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */ + matrix::Vector3f _vel_int; /**< integral term of the velocity controller */ + float _yaw{}; /**< current heading */ + + // Setpoints + matrix::Vector3f _pos_sp; /**< desired position */ + matrix::Vector3f _vel_sp; /**< desired velocity */ + matrix::Vector3f _acc_sp; /**< desired acceleration */ + matrix::Vector3f _thr_sp; /**< desired thrust */ + float _yaw_sp{}; /**< desired heading */ + float _yawspeed_sp{}; /** desired yaw-speed */ +}; diff --git a/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp new file mode 100644 index 0000000..f2bb5cf --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -0,0 +1,385 @@ +/**************************************************************************** + * + * 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 +#include +#include + +using namespace matrix; + +TEST(PositionControlTest, EmptySetpoint) +{ + PositionControl position_control; + + vehicle_local_position_setpoint_s output_setpoint{}; + position_control.getLocalPositionSetpoint(output_setpoint); + EXPECT_FLOAT_EQ(output_setpoint.x, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.y, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.z, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.yaw, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.yawspeed, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.vx, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.vy, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.vz, 0.f); + EXPECT_EQ(Vector3f(output_setpoint.acceleration), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(output_setpoint.jerk), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(output_setpoint.thrust), Vector3f(0, 0, 0)); + + vehicle_attitude_setpoint_s attitude{}; + position_control.getAttitudeSetpoint(attitude); + EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); + EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(attitude.roll_reset_integral, false); + EXPECT_EQ(attitude.pitch_reset_integral, false); + EXPECT_EQ(attitude.yaw_reset_integral, false); + EXPECT_EQ(attitude.fw_control_yaw, false); + EXPECT_FLOAT_EQ(attitude.apply_flaps, 0.f);//vehicle_attitude_setpoint_s::FLAPS_OFF); // TODO why no reference? +} + +class PositionControlBasicTest : public ::testing::Test +{ +public: + PositionControlBasicTest() + { + _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f)); + _position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f)); + _position_control.setVelocityLimits(1.f, 1.f, 1.f); + _position_control.setThrustLimits(0.1f, 0.9f); + _position_control.setTiltLimit(1.f); + _position_control.setHoverThrust(.5f); + + resetInputSetpoint(); + } + + void resetInputSetpoint() + { + _input_setpoint.x = NAN; + _input_setpoint.y = NAN; + _input_setpoint.z = NAN; + _input_setpoint.yaw = NAN; + _input_setpoint.yawspeed = NAN; + _input_setpoint.vx = NAN; + _input_setpoint.vy = NAN; + _input_setpoint.vz = NAN; + Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.acceleration); + Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust); + } + + bool runController() + { + _position_control.setInputSetpoint(_input_setpoint); + const bool ret = _position_control.update(.1f); + _position_control.getLocalPositionSetpoint(_output_setpoint); + _position_control.getAttitudeSetpoint(_attitude); + return ret; + } + + PositionControl _position_control; + vehicle_local_position_setpoint_s _input_setpoint{}; + vehicle_local_position_setpoint_s _output_setpoint{}; + vehicle_attitude_setpoint_s _attitude{}; +}; + +class PositionControlBasicDirectionTest : public PositionControlBasicTest +{ +public: + void checkDirection() + { + Vector3f thrust(_output_setpoint.thrust); + EXPECT_GT(thrust(0), 0.f); + EXPECT_GT(thrust(1), 0.f); + EXPECT_LT(thrust(2), 0.f); + + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + EXPECT_LT(body_z(0), 0.f); + EXPECT_LT(body_z(1), 0.f); + EXPECT_GT(body_z(2), 0.f); + } +}; + +TEST_F(PositionControlBasicDirectionTest, PositionDirection) +{ + _input_setpoint.x = .1f; + _input_setpoint.y = .1f; + _input_setpoint.z = -.1f; + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicDirectionTest, VelocityDirection) +{ + _input_setpoint.vx = .1f; + _input_setpoint.vy = .1f; + _input_setpoint.vz = -.1f; + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicTest, TiltLimit) +{ + _input_setpoint.x = 10.f; + _input_setpoint.y = 10.f; + _input_setpoint.z = -0.f; + + EXPECT_TRUE(runController()); + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, 1.f); + + _position_control.setTiltLimit(0.5f); + EXPECT_TRUE(runController()); + body_z = Quatf(_attitude.q_d).dcm_z(); + angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, .50001f); + + _position_control.setTiltLimit(1.f); // restore original +} + +TEST_F(PositionControlBasicTest, VelocityLimit) +{ + _input_setpoint.x = 10.f; + _input_setpoint.y = 10.f; + _input_setpoint.z = -10.f; + + EXPECT_TRUE(runController()); + Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy); + EXPECT_LE(velocity_xy.norm(), 1.f); + EXPECT_LE(abs(_output_setpoint.vz), 1.f); +} + +TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) +{ + _input_setpoint.x = 10.f; + _input_setpoint.y = 10.f; + _input_setpoint.z = -10.f; + + runController(); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust(0), 0.f); + EXPECT_FLOAT_EQ(thrust(1), 0.f); + EXPECT_FLOAT_EQ(thrust(2), -0.9f); + + EXPECT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.9f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); +} + +TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) +{ + _input_setpoint.x = 10.f; + _input_setpoint.y = 0.f; + _input_setpoint.z = 10.f; + + runController(); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust.length(), 0.1f); + + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); +} + +TEST_F(PositionControlBasicTest, FailsafeInput) +{ + _input_setpoint.vz = .1f; + _input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f; + _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_LT(_output_setpoint.thrust[2], -.1f); + EXPECT_GT(_output_setpoint.thrust[2], -.5f); + EXPECT_GT(_attitude.thrust_body[2], -.5f); + EXPECT_LE(_attitude.thrust_body[2], -.1f); +} + +TEST_F(PositionControlBasicTest, IdleThrustInput) +{ + // High downwards acceleration to make sure there's no thrust + Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); +} + +TEST_F(PositionControlBasicTest, InputCombinationsPosition) +{ + _input_setpoint.x = .1f; + _input_setpoint.y = .2f; + _input_setpoint.z = .3f; + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.x, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.y, .2f); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FALSE(isnan(_output_setpoint.vx)); + EXPECT_FALSE(isnan(_output_setpoint.vy)); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) +{ + _input_setpoint.vx = .1f; + _input_setpoint.vy = .2f; + _input_setpoint.z = .3f; // altitude + + EXPECT_TRUE(runController()); + // EXPECT_TRUE(isnan(_output_setpoint.x)); + // EXPECT_TRUE(isnan(_output_setpoint.y)); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, SetpointValiditySimple) +{ + EXPECT_FALSE(runController()); + _input_setpoint.x = .1f; + EXPECT_FALSE(runController()); + _input_setpoint.y = .2f; + EXPECT_FALSE(runController()); + _input_setpoint.acceleration[2] = .3f; + EXPECT_TRUE(runController()); +} + +TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) +{ + // This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly + float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.acceleration[0], + &_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.acceleration[1], + &_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.acceleration[2] + }; + + for (int combination = 0; combination < 512; combination++) { + resetInputSetpoint(); + + for (int j = 0; j < 9; j++) { + if (combination & (1 << j)) { + // Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations + *(setpoint_loop_access_map[j]) = static_cast(combination) / static_cast(j + 1); + } + } + + // Expect at least one setpoint per axis + const bool has_x_setpoint = ((combination & 7) != 0); + const bool has_y_setpoint = (((combination >> 3) & 7) != 0); + const bool has_z_setpoint = (((combination >> 6) & 7) != 0); + // Expect xy setpoints to come in pairs + const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7); + const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs; + + EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl + << "input" << std::endl + << "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl + << "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl + << "acceleration " << _input_setpoint.acceleration[0] << ", " + << _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl + << "output" << std::endl + << "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl + << "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl + << "acceleration " << _output_setpoint.acceleration[0] << ", " + << _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl; + } +} + +TEST_F(PositionControlBasicTest, InvalidState) +{ + _input_setpoint.x = .1f; + _input_setpoint.y = .2f; + _input_setpoint.z = .3f; + + PositionControlStates states{}; + states.position(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.position(0) = 0.f; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = 0.f; + states.acceleration(1) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); +} + + +TEST_F(PositionControlBasicTest, UpdateHoverThrust) +{ + // GIVEN: some hover thrust and 0 velocity change + const float hover_thrust = 0.6f; + _position_control.setHoverThrust(hover_thrust); + + _input_setpoint.vx = 0.f; + _input_setpoint.vy = 0.f; + _input_setpoint.vz = -0.f; + + // WHEN: we run the controller + EXPECT_TRUE(runController()); + + // THEN: the output thrust equals the hover thrust + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); + + // HOWEVER WHEN: we set a new hover thrust through the update function + const float hover_thrust_new = 0.7f; + _position_control.updateHoverThrust(hover_thrust_new); + EXPECT_TRUE(runController()); + + // THEN: the integral is updated to avoid discontinuities and + // the output is still the same + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); +} diff --git a/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/CMakeLists.txt b/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/CMakeLists.txt new file mode 100644 index 0000000..a4c129a --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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(Takeoff + Takeoff.cpp + Takeoff.hpp +) +target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(Takeoff PUBLIC hysteresis) + +px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) diff --git a/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/Takeoff.cpp new file mode 100644 index 0000000..3717106 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 Takeoff.cpp + */ + +#include "Takeoff.hpp" +#include +#include + +void Takeoff::generateInitialRampValue(float velocity_p_gain) +{ + velocity_p_gain = math::max(velocity_p_gain, 0.01f); + _takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain; +} + +void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, + const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us) +{ + _spoolup_time_hysteresis.set_state_and_update(armed, now_us); + + switch (_takeoff_state) { + case TakeoffState::disarmed: + if (armed) { + _takeoff_state = TakeoffState::spoolup; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::spoolup: + if (_spoolup_time_hysteresis.get_state()) { + _takeoff_state = TakeoffState::ready_for_takeoff; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::ready_for_takeoff: + if (want_takeoff) { + _takeoff_state = TakeoffState::rampup; + _takeoff_ramp_progress = 0.f; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::rampup: + if (_takeoff_ramp_progress >= 1.f) { + _takeoff_state = TakeoffState::flight; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::flight: + if (landed) { + _takeoff_state = TakeoffState::ready_for_takeoff; + } + + break; + + default: + break; + } + + if (armed && skip_takeoff) { + _takeoff_state = TakeoffState::flight; + } + + // TODO: need to consider free fall here + if (!armed) { + _takeoff_state = TakeoffState::disarmed; + } +} + +float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz) +{ + float upwards_velocity_limit = takeoff_desired_vz; + + if (_takeoff_state < TakeoffState::rampup) { + upwards_velocity_limit = _takeoff_ramp_vz_init; + } + + if (_takeoff_state == TakeoffState::rampup) { + if (_takeoff_ramp_time > dt) { + _takeoff_ramp_progress += dt / _takeoff_ramp_time; + + } else { + _takeoff_ramp_progress = 1.f; + } + + if (_takeoff_ramp_progress < 1.f) { + upwards_velocity_limit = _takeoff_ramp_vz_init + _takeoff_ramp_progress * (takeoff_desired_vz - _takeoff_ramp_vz_init); + } + } + + return upwards_velocity_limit; +} diff --git a/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/Takeoff.hpp new file mode 100644 index 0000000..ab2a39f --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 Takeoff.hpp + * + * A class handling all takeoff states and a smooth ramp up of the motors. + */ + +#pragma once + +#include +#include +#include + +using namespace time_literals; + +enum class TakeoffState { + disarmed = takeoff_status_s::TAKEOFF_STATE_DISARMED, + spoolup = takeoff_status_s::TAKEOFF_STATE_SPOOLUP, + ready_for_takeoff = takeoff_status_s::TAKEOFF_STATE_READY_FOR_TAKEOFF, + rampup = takeoff_status_s::TAKEOFF_STATE_RAMPUP, + flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT +}; + +class Takeoff +{ +public: + Takeoff() = default; + ~Takeoff() = default; + + // initialize parameters + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); } + void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } + + /** + * Calculate a vertical velocity to initialize the takeoff ramp + * that when passed to the velocity controller results in a zero throttle setpoint. + * @param hover_thrust normalized thrsut value with which the vehicle hovers + * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust + */ + void generateInitialRampValue(const float velocity_p_gain); + + /** + * Update the state for the takeoff. + * @param setpoint a vehicle_local_position_setpoint_s structure + * @return true if setpoint has updated correctly + */ + void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, + const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us); + + /** + * Update and return the velocity constraint ramp value during takeoff. + * By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved. + * Returns zero on the ground and takeoff_desired_vz in flight. + * @param dt time in seconds since the last call/loop iteration + * @param takeoff_desired_vz end value for the velocity ramp + * @return true if setpoint has updated correctly + */ + float updateRamp(const float dt, const float takeoff_desired_vz); + + TakeoffState getTakeoffState() { return _takeoff_state; } + +private: + TakeoffState _takeoff_state = TakeoffState::disarmed; + + systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed + + float _takeoff_ramp_time{0.f}; + float _takeoff_ramp_vz_init{0.f}; ///< verticval velocity resulting in zero thrust + float _takeoff_ramp_progress{0.f}; ///< asecnding from 0 to 1 +}; diff --git a/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp b/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp new file mode 100644 index 0000000..c326233 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include + +TEST(TakeoffTest, Initialization) +{ + Takeoff takeoff; + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); +} + +TEST(TakeoffTest, RegularTakeoffRamp) +{ + Takeoff takeoff; + takeoff.setSpoolupTime(1.f); + takeoff.setTakeoffRampTime(2.0); + takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f); + + // disarmed, landed, don't want takeoff + takeoff.updateTakeoffState(false, true, false, 1.f, false, 0); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); + + // armed, not landed anymore, don't want takeoff + takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup); + + // armed, not landed, don't want takeoff yet, spoolup time passed + takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff); + + // armed, not landed, want takeoff + takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup); + + // armed, not landed, want takeoff, ramping up + takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); + + // armed, not landed, want takeoff, rampup time passed + takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight); +} diff --git a/src/prometheus_px4/src/modules/mc_pos_control/mc_pos_control_params.c b/src/prometheus_px4/src/modules/mc_pos_control/mc_pos_control_params.c new file mode 100644 index 0000000..46c5fb3 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_pos_control/mc_pos_control_params.c @@ -0,0 +1,785 @@ +/**************************************************************************** + * + * 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 mc_pos_control_params.c + * Multicopter position controller parameters. + * + * @author Anton Babushkin + */ + +/** + * Minimum thrust in auto thrust control + * + * It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @unit norm + * @min 0.05 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); + +/** + * Hover thrust + * + * Vertical thrust required to hover. + * This value is mapped to center stick for manual throttle control. + * With this value set to the thrust required to hover, transition + * from manual to Altitude or Position mode while hovering will occur with the + * throttle stick near center, which is then interpreted as (near) + * zero demand for vertical speed. + * + * This parameter is also important for the landing detection to work correctly. + * + * @unit norm + * @min 0.1 + * @max 0.8 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); + +/** + * Hover thrust source selector + * + * Set false to use the fixed parameter MPC_THR_HOVER + * Set true to use the value computed by the hover thrust estimator + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_USE_HTE, 1); + +/** + * Thrust curve in Manual Mode + * + * This parameter defines how the throttle stick input is mapped to commanded thrust + * in Manual/Stabilized flight mode. + * + * In case the default is used ('Rescale to hover thrust'), the stick input is linearly + * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). + * + * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful + * in case the hover thrust is very low and the default would lead to too much distortion + * (e.g. if hover thrust is set to 20%, 80% of the upper thrust range is squeezed into the + * upper half of the stick range). + * + * Note: In case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. + * + * @value 0 Rescale to hover thrust + * @value 1 No Rescale + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_THR_CURVE, 0); + +/** + * Maximum thrust in auto thrust control + * + * Limit max allowed thrust + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); + +/** + * Minimum manual thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * With MC_AIRMODE set to 1, this can safely be set to 0. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @max 1.5 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); + +/** + * Proportional gain for vertical velocity error + * + * defined as correction acceleration in m/s^2 per m/s velocity error + * + * @min 2.0 + * @max 15.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.0f); + +/** + * Integral gain for vertical velocity error + * + * defined as correction acceleration in m/s^2 per m velocity integral + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.2 + * @max 3.0 + * @decimal 3 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f); + +/** + * Differential gain for vertical velocity error + * + * defined as correction acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0.0 + * @max 2.0 + * @decimal 3 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f); + +/** + * Maximum vertical ascent velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). + * + * @unit m/s + * @min 0.5 + * @max 8.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.0f); + +/** + * Maximum vertical descent velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). + * + * @unit m/s + * @min 0.5 + * @max 4.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.0f); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f); + +/** + * Proportional gain for horizontal velocity error + * + * defined as correction acceleration in m/s^2 per m/s velocity error + * + * @min 1.2 + * @max 5.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f); + +/** + * Integral gain for horizontal velocity error + * + * defined as correction acceleration in m/s^2 per m velocity integral + * Non-zero value allows to eliminate steady state errors in the presence of disturbances like wind. + * + * @min 0.0 + * @max 60.0 + * @decimal 3 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * defined as correction acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0.1 + * @max 2.0 + * @decimal 3 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f); + +/** + * Maximum horizontal velocity in mission + * + * Horizontal velocity used when flying autonomously in e.g. Missions, RTL, Goto. + * + * @unit m/s + * @min 3.0 + * @max 20.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); + +/** + * Proportional gain for horizontal trajectory position error + * + * @min 0.1 + * @max 1.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f); + +/** + * Maximum horizontal error allowed by the trajectory generator + * + * The integration speed of the trajectory setpoint is linearly + * reduced with the horizontal position tracking error. When the + * error is above this parameter, the integration of the + * trajectory is stopped to wait for the drone. + * + * This value can be adjusted depending on the tracking + * capabilities of the vehicle. + * + * @min 0.1 + * @max 10.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f); + +/** + * Maximum horizontal velocity setpoint for manual controlled mode + * + * If velocity setpoint larger than MPC_XY_VEL_MAX is set, then + * the setpoint will be capped to MPC_XY_VEL_MAX + * + * @unit m/s + * @min 3.0 + * @max 20.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.0f); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode. If higher speeds + * are commanded in a mission they will be capped to this velocity. + * + * @unit m/s + * @min 0.0 + * @max 20.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.0f); + +/** + * Maximum tilt angle in air + * + * Limits maximum tilt in AUTO and POSCTRL modes during flight. + * + * @unit deg + * @min 20.0 + * @max 89.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 10.0 + * @max 89.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.6 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); + +/** + * Enable user assisted descent speed for autonomous land routine. + * + * When enabled, descent speed will be: + * stick full up - 0 + * stick centered - MPC_LAND_SPEED + * stick full down - 2 * MPC_LAND_SPEED + * + * @min 0 + * @max 1 + * @value 0 Fixed descent speed of MPC_LAND_SPEED + * @value 1 User assisted descent speed + */ +PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); + +/** + * Takeoff climb rate + * + * @unit m/s + * @min 1 + * @max 5 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); + +/** + * Maximal tilt angle in manual or altitude mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_TILT_MAX, 35.0f); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 400 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 150.0f); + +/** + * Manual yaw rate input filter time constant + * + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0.0 + * @max 5.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); + +/** + * Deadzone of sticks where position hold is enabled + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f); + +/** + * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @max 3.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.8f); + +/** + * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @max 3.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.6f); + +/** + * Low pass filter cut freq. for numerical velocity derivative + * + * @unit Hz + * @min 0.0 + * @max 10 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); + +/** + * Maximum horizontal acceleration for auto mode and for manual mode + * + * MPC_POS_MODE + * 1 just deceleration + * 3 acceleration and deceleration + * 4 just acceleration + * + * @unit m/s^2 + * @min 2.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.0f); + +/** + * Acceleration for auto and for manual + * + * Note: In manual, this parameter is only used in MPC_POS_MODE 4. + * + * @unit m/s^2 + * @min 2.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ + +PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.0f); + +/** + * Maximum vertical acceleration in velocity controlled modes upward + * + * @unit m/s^2 + * @min 2.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.0f); + +/** + * Maximum vertical acceleration in velocity controlled modes down + * + * @unit m/s^2 + * @min 2.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.0f); + +/** + * Maximum jerk limit + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions, but it also limits its + * agility (how fast it can change directions or break). + * + * Setting this to the maximum value essentially disables the limit. + * + * Note: This is only used when MPC_POS_MODE is set to a smoothing mode 3 or 4. + * + * @unit m/s^3 + * @min 0.5 + * @max 500.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 8.0f); + +/** + * Jerk limit in auto mode + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions, but it also limits its + * agility. + * + * @unit m/s^3 + * @min 1.0 + * @max 80.0 + * @increment 1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_AUTO, 4.0f); + +/** + * Altitude control mode. + * + * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in + * flight due to sensor drift. + * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down + * with terrain height variation. Requires a distance to ground sensor. The height controller will + * revert to using height above origin if the distance to ground estimate becomes invalid as indicated + * by the local_position.distance_bottom_valid message being false. + * Set to 2 to control height relative to ground (requires a distance sensor) when stationary and relative + * to earth frame origin when moving horizontally. + * The speed threshold is controlled by the MPC_HOLD_MAX_XY parameter. + * + * @min 0 + * @max 2 + * @value 0 Altitude following + * @value 1 Terrain following + * @value 2 Terrain hold + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); + +/** + * Manual position control stick exponential curve sensitivity + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve (default) + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.6f); + +/** + * Manual control stick vertical exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve (default) + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.6f); + +/** + * Manual control stick yaw rotation exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve (default) + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_YAW_EXPO, 0.6f); + +/** + * Max yaw rate in auto mode + * + * Limit the rate of change of the yaw setpoint in autonomous mode + * to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f); + +/** + * Altitude for 1. step of slow landing (descend) + * + * Below this altitude descending velocity gets limited to a value + * between "MPC_Z_VEL_MAX_DN" and "MPC_LAND_SPEED" + * Value needs to be higher than "MPC_LAND_ALT2" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f); + +/** + * Altitude for 2. step of slow landing (landing) + * + * Below this altitude descending velocity gets + * limited to "MPC_LAND_SPEED". + * Value needs to be lower than "MPC_LAND_ALT1" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); + +/** + * Position control smooth takeoff ramp time constant + * + * Increasing this value will make automatic and manual takeoff slower. + * If it's too slow the drone might scratch the ground and tip over. + * A time constant of 0 disables the ramp + * + * @min 0 + * @max 5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f); + +/** + * Manual-Position control sub-mode + * + * The supported sub-modes are: + * 0 Simple position control where sticks map directly to velocity setpoints + * without smoothing. Useful for velocity control tuning. + * 3 Smooth position control with maximum acceleration and jerk limits based on + * jerk optimized trajectory generator (different algorithm than 1). + * 4 Smooth position control where sticks map to acceleration and there's a virtual brake drag + * + * @value 0 Simple position control + * @value 3 Smooth position control (Jerk optimized) + * @value 4 Acceleration based input + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_POS_MODE, 4); + +/** + * Enforced delay between arming and takeoff + * + * For altitude controlled modes the time from arming the motors until + * a takeoff is possible gets forced to be at least MPC_SPOOLUP_TIME seconds + * to ensure the motors and propellers can sppol up and reach idle speed before + * getting commanded to spin faster. This delay is particularly useful for vehicles + * with slow motor spin-up e.g. because of large propellers. + * + * @min 0 + * @max 10 + * @unit s + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); + +/** + * Yaw mode. + * + * Specifies the heading in Auto. + * + * @min 0 + * @max 4 + * @value 0 towards waypoint + * @value 1 towards home + * @value 2 away from home + * @value 3 along trajectory + * @value 4 towards waypoint (yaw first) + * @group Mission + */ +PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); + +/** + * Responsiveness + * + * Changes the overall responsiveness of the vehicle. + * The higher the value, the faster the vehicle will react. + * + * If set to a value greater than zero, other parameters are automatically set (such as + * the acceleration or jerk limits). + * If set to a negative value, the existing individual parameters are used. + * + * @min -1 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SYS_VEHICLE_RESP, -0.4f); + +/** + * Overall Horizonal Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_XY_VEL_MAX or MPC_VEL_MANUAL). + * If set to a negative value, the existing individual parameters are used. + * + * @min -20 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_ALL, -10.0f); + +/** + * Overall Vertical Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_Z_VEL_MAX_UP or MPC_LAND_SPEED). + * If set to a negative value, the existing individual parameters are used. + * + * @min -3 + * @max 8 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_ALL, -3.0f); diff --git a/src/prometheus_px4/src/modules/mc_rate_control/CMakeLists.txt b/src/prometheus_px4/src/modules/mc_rate_control/CMakeLists.txt new file mode 100644 index 0000000..758d56b --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_rate_control/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# 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(RateControl) + +px4_add_module( + MODULE modules__mc_rate_control + MAIN mc_rate_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + MulticopterRateControl.cpp + MulticopterRateControl.hpp + DEPENDS + circuit_breaker + mathlib + RateControl + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/prometheus_px4/src/modules/mc_rate_control/MulticopterRateControl.cpp new file mode 100644 index 0000000..4e907fc --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -0,0 +1,335 @@ +/**************************************************************************** + * + * 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 "MulticopterRateControl.hpp" + +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +MulticopterRateControl::MulticopterRateControl(bool vtol) : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + parameters_updated(); +} + +MulticopterRateControl::~MulticopterRateControl() +{ + perf_free(_loop_perf); +} + +bool +MulticopterRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; + } + + return true; +} + +void +MulticopterRateControl::parameters_updated() +{ + // rate 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 rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); + + _rate_control.setGains( + rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); + + _rate_control.setIntegratorLimit( + Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); + + _rate_control.setFeedForwardGain( + Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); + + + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), + radians(_param_mc_acro_y_max.get())); + + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY); +} + +void +MulticopterRateControl::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(¶m_update); + + updateParams(); + parameters_updated(); + } + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + + // grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy + vehicle_angular_acceleration_s v_angular_acceleration{}; + _vehicle_angular_acceleration_sub.copy(&v_angular_acceleration); + + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f angular_accel{v_angular_acceleration.xyz}; + const Vector3f rates{angular_velocity.xyz}; + + /* check for updates in other topics */ + _v_control_mode_sub.update(&_v_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; + } + } + + _vehicle_status_sub.update(&_vehicle_status); + + if (_landing_gear_sub.updated()) { + landing_gear_s landing_gear; + + if (_landing_gear_sub.copy(&landing_gear)) { + if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { + _landing_gear = landing_gear.landing_gear; + } + } + } + + if (_v_control_mode.flag_control_manual_enabled && !_v_control_mode.flag_control_attitude_enabled) { + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{ + math::superexpo(manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; + + _rates_sp = man_rate_sp.emult(_acro_rate_max); + _thrust_sp = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f); + + // publish rate setpoint + vehicle_rates_setpoint_s v_rates_sp{}; + v_rates_sp.roll = _rates_sp(0); + v_rates_sp.pitch = _rates_sp(1); + v_rates_sp.yaw = _rates_sp(2); + v_rates_sp.thrust_body[0] = 0.0f; + v_rates_sp.thrust_body[1] = 0.0f; + v_rates_sp.thrust_body[2] = -_thrust_sp; + v_rates_sp.timestamp = hrt_absolute_time(); + + _v_rates_sp_pub.publish(v_rates_sp); + } + + } else { + // use rates setpoint topic + vehicle_rates_setpoint_s v_rates_sp; + + if (_v_rates_sp_sub.update(&v_rates_sp)) { + _rates_sp(0) = PX4_ISFINITE(v_rates_sp.roll) ? v_rates_sp.roll : rates(0); + _rates_sp(1) = PX4_ISFINITE(v_rates_sp.pitch) ? v_rates_sp.pitch : rates(1); + _rates_sp(2) = PX4_ISFINITE(v_rates_sp.yaw) ? v_rates_sp.yaw : rates(2); + _thrust_sp = -v_rates_sp.thrust_body[2]; + } + } + + // run the rate controller + if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) { + + // reset integral if disarmed + if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _rate_control.resetIntegral(); + } + + // update saturation status from mixer feedback + if (_motor_limits_sub.updated()) { + multirotor_motor_limits_s motor_limits; + + if (_motor_limits_sub.copy(&motor_limits)) { + MultirotorMixer::saturation_status saturation_status; + saturation_status.value = motor_limits.saturation_status; + + _rate_control.setSaturationStatus(saturation_status); + } + } + + // run rate controller + const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish actuator controls + actuator_controls_s actuators{}; + actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f; + actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f; + actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f; + actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f; + actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear; + actuators.timestamp_sample = angular_velocity.timestamp_sample; + + // scale effort by battery status if enabled + if (_param_mc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.0f) { + for (int i = 0; i < 4; i++) { + actuators.control[i] *= _battery_status_scale; + } + } + } + + actuators.timestamp = hrt_absolute_time(); + _actuators_0_pub.publish(actuators); + + } else if (_v_control_mode.flag_control_termination_enabled) { + if (!_vehicle_status.is_vtol) { + // publish actuator controls + actuator_controls_s actuators{}; + actuators.timestamp = hrt_absolute_time(); + _actuators_0_pub.publish(actuators); + } + } + } + + perf_end(_loop_perf); +} + +int MulticopterRateControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + MulticopterRateControl *instance = new MulticopterRateControl(vtol); + + 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 MulticopterRateControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the multicopter rate controller. It takes rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. + +The controller has a PID loop for angular rate error. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]) +{ + return MulticopterRateControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/prometheus_px4/src/modules/mc_rate_control/MulticopterRateControl.hpp new file mode 100644 index 0000000..531b1e4 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class MulticopterRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + MulticopterRateControl(bool vtol = false); + ~MulticopterRateControl() 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; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + RateControl _rate_control; ///< class for rate control calculations + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::Publication _actuators_0_pub; + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ + uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + + vehicle_control_mode_s _v_control_mode{}; + vehicle_status_s _vehicle_status{}; + + bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ + bool _landed{true}; + bool _maybe_landed{true}; + + float _battery_status_scale{0.0f}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + matrix::Vector3f _rates_sp; /**< angular rates setpoint */ + + float _thrust_sp{0.0f}; /**< thrust setpoint */ + + hrt_abstime _last_run{0}; + + int8_t _landing_gear{landing_gear_s::GEAR_DOWN}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mc_rollrate_p, + (ParamFloat) _param_mc_rollrate_i, + (ParamFloat) _param_mc_rr_int_lim, + (ParamFloat) _param_mc_rollrate_d, + (ParamFloat) _param_mc_rollrate_ff, + (ParamFloat) _param_mc_rollrate_k, + + (ParamFloat) _param_mc_pitchrate_p, + (ParamFloat) _param_mc_pitchrate_i, + (ParamFloat) _param_mc_pr_int_lim, + (ParamFloat) _param_mc_pitchrate_d, + (ParamFloat) _param_mc_pitchrate_ff, + (ParamFloat) _param_mc_pitchrate_k, + + (ParamFloat) _param_mc_yawrate_p, + (ParamFloat) _param_mc_yawrate_i, + (ParamFloat) _param_mc_yr_int_lim, + (ParamFloat) _param_mc_yawrate_d, + (ParamFloat) _param_mc_yawrate_ff, + (ParamFloat) _param_mc_yawrate_k, + + (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ + + (ParamFloat) _param_mc_acro_r_max, + (ParamFloat) _param_mc_acro_p_max, + (ParamFloat) _param_mc_acro_y_max, + (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamBool) _param_mc_bat_scale_en, + + (ParamInt) _param_cbrk_rate_ctrl + ) + + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + +}; diff --git a/src/prometheus_px4/src/modules/mc_rate_control/RateControl/CMakeLists.txt b/src/prometheus_px4/src/modules/mc_rate_control/RateControl/CMakeLists.txt new file mode 100644 index 0000000..b536a7d --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_rate_control/RateControl/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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(RateControl + RateControl.cpp + RateControl.hpp +) +target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(RateControl PRIVATE mathlib) + +px4_add_unit_gtest(SRC RateControlTest.cpp LINKLIBS RateControl) diff --git a/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControl.cpp new file mode 100644 index 0000000..37a2525 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 RateControl.cpp + */ + +#include +#include + +using namespace matrix; + +void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_p = P; + _gain_i = I; + _gain_d = D; +} + +void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status) +{ + _mixer_saturation_positive[0] = status.flags.roll_pos; + _mixer_saturation_positive[1] = status.flags.pitch_pos; + _mixer_saturation_positive[2] = status.flags.yaw_pos; + _mixer_saturation_negative[0] = status.flags.roll_neg; + _mixer_saturation_negative[1] = status.flags.pitch_neg; + _mixer_saturation_negative[2] = status.flags.yaw_neg; +} + +Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel, + const float dt, const bool landed) +{ + // angular rates error + Vector3f rate_error = rate_sp - rate; + + // PID control with feed forward + const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); + + // update integral only if we are not landed + if (!landed) { + updateIntegral(rate_error, dt); + } + + return torque; +} + +void RateControl::updateIntegral(Vector3f &rate_error, const float dt) +{ + for (int i = 0; i < 3; i++) { + // prevent further positive control saturation + if (_mixer_saturation_positive[i]) { + rate_error(i) = math::min(rate_error(i), 0.f); + } + + // prevent further negative control saturation + if (_mixer_saturation_negative[i]) { + rate_error(i) = math::max(rate_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 = rate_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 rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt; + + // do not propagate the result if out of range or invalid + if (PX4_ISFINITE(rate_i)) { + _rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i)); + } + } +} + +void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status) +{ + rate_ctrl_status.rollspeed_integ = _rate_int(0); + rate_ctrl_status.pitchspeed_integ = _rate_int(1); + rate_ctrl_status.yawspeed_integ = _rate_int(2); +} diff --git a/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControl.hpp new file mode 100644 index 0000000..a39dabb --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 RateControl.hpp + * + * PID 3 axis angular rate / angular velocity control. + */ + +#pragma once + +#include + +#include +#include + +class RateControl +{ +public: + RateControl() = default; + ~RateControl() = default; + + /** + * Set the rate 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 rate 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 saturation status + * @param status message from mixer reporting about saturation + */ + void setSaturationStatus(const MultirotorMixer::saturation_status &status); + + /** + * Run one control loop cycle calculation + * @param rate estimation of the current vehicle angular rate + * @param rate_sp desired vehicle angular rate setpoint + * @param dt desired vehicle angular rate setpoint + * @return [-1,1] normalized torque vector to apply to the vehicle + */ + matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, + const matrix::Vector3f &angular_accel, const float dt, const bool landed); + + /** + * Set the integral term to 0 to prevent windup + * @see _rate_int + */ + void resetIntegral() { _rate_int.zero(); } + + /** + * Get status message of controller for logging/debugging + * @param rate_ctrl_status status message to fill with internal states + */ + void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); + +private: + void updateIntegral(matrix::Vector3f &rate_error, const float dt); + + // Gains + matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z + matrix::Vector3f _gain_i; ///< rate control integral gain + matrix::Vector3f _gain_d; ///< rate control derivative gain + matrix::Vector3f _lim_int; ///< integrator term maximum absolute value + matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters + + // States + matrix::Vector3f _rate_int; ///< integral term of the rate controller + + bool _mixer_saturation_positive[3] {}; + bool _mixer_saturation_negative[3] {}; +}; diff --git a/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControlTest.cpp b/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControlTest.cpp new file mode 100644 index 0000000..791fb20 --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_rate_control/RateControl/RateControlTest.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * 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 +#include + +using namespace matrix; + +TEST(RateControlTest, AllZeroCase) +{ + RateControl rate_control; + Vector3f torque = rate_control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false); + EXPECT_EQ(torque, Vector3f()); +} diff --git a/src/prometheus_px4/src/modules/mc_rate_control/mc_rate_control_params.c b/src/prometheus_px4/src/modules/mc_rate_control/mc_rate_control_params.c new file mode 100644 index 0000000..4bc092d --- /dev/null +++ b/src/prometheus_px4/src/modules/mc_rate_control/mc_rate_control_params.c @@ -0,0 +1,400 @@ +/**************************************************************************** + * + * 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 mc_rate_control_params.c + * Parameters for multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.5 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f); + +/** + * Roll rate integrator limit + * + * Roll rate 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. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @max 0.01 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); + +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + +/** + * Roll rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error + * + MC_ROLLRATE_I * error_integral + * + MC_ROLLRATE_D * error_derivative) + * Set MC_ROLLRATE_P=1 to implement a PID in the ideal form. + * Set MC_ROLLRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.6 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f); + +/** + * Pitch rate integrator limit + * + * Pitch rate 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. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); + +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + +/** + * Pitch rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error + * + MC_PITCHRATE_I * error_integral + * + MC_PITCHRATE_D * error_derivative) + * Set MC_PITCHRATE_P=1 to implement a PID in the ideal form. + * Set MC_PITCHRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @max 0.6 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); + +/** + * Yaw rate integrator limit + * + * Yaw rate 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. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); + +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + +/** + * Yaw rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_YAWRATE_K * (MC_YAWRATE_P * error + * + MC_YAWRATE_I * error_integral + * + MC_YAWRATE_D * error_derivative) + * Set MC_YAWRATE_P=1 to implement a PID in the ideal form. + * Set MC_YAWRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); + +/** + * Max acro roll rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f); + +/** + * Max acro pitch rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f); + +/** + * Max acro yaw rate + * + * default 1.5 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f); + +/** + * Acro mode Expo factor for Roll and Pitch. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); + +/** + * Acro mode Expo factor for Yaw. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f); + +/** + * Acro mode SuperExpo factor for Roll and Pitch. + * + * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO. + * + * 0 Pure Expo function + * 0.7 resonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); + +/** + * Acro mode SuperExpo factor for Yaw. + * + * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. + * + * 0 Pure Expo function + * 0.7 resonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); + +/** + * Battery power level scaler + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The copter + * should constantly behave as if it was fully charged with reduced max acceleration + * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group Multicopter Rate Control + */ +PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/CMakeLists.txt new file mode 100644 index 0000000..d4ecbfd --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/CMakeLists.txt @@ -0,0 +1,183 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +#============================================================================= +# RTPS and micro-cdr +# + +find_program(FASTRTPSGEN fastrtpsgen PATHS $ENV{FASTRTPSGEN_DIR}) +if(NOT FASTRTPSGEN) + message(FATAL_ERROR "Unable to find fastrtpsgen") +else() + execute_process( + COMMAND $ENV{FASTRTPSGEN_DIR}fastrtpsgen -version + OUTPUT_VARIABLE FASTRTPSGEN_VERSION + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_QUIET + ) + message(STATUS "${FASTRTPSGEN_VERSION}") +endif() + +if (EXISTS "${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_message_ids.yaml") + set(config_rtps_send_topics) + execute_process( + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -sa + OUTPUT_VARIABLE config_rtps_send_topics + ) + set(config_rtps_send_alias_topics "") + string(FIND ${config_rtps_send_topics} "alias" found_send_alias) + if (NOT ${found_send_alias} EQUAL "-1") + STRING(REGEX REPLACE ".*alias " "" config_rtps_send_alias_topics "${config_rtps_send_topics}") + STRING(REPLACE ", " ";" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}") + STRING(REPLACE "\n" "" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}") + STRING(REGEX REPLACE " alias.*" "" config_rtps_send_topics "${config_rtps_send_topics}") + endif() + STRING(REGEX REPLACE ", " ";" config_rtps_send_topics "${config_rtps_send_topics}") + STRING(REGEX REPLACE "\n" "" config_rtps_send_topics "${config_rtps_send_topics}") + + set(config_rtps_receive_topics) + execute_process( + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -ra + OUTPUT_VARIABLE config_rtps_receive_topics + ) + set(config_rtps_receive_alias_topics "") + string(FIND ${config_rtps_receive_topics} "alias" found_receive_alias) + if (NOT ${found_receive_alias} EQUAL "-1") + STRING(REGEX REPLACE ".*alias " "" config_rtps_receive_alias_topics "${config_rtps_receive_topics}") + STRING(REPLACE ", " ";" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}") + STRING(REPLACE "\n" "" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}") + STRING(REGEX REPLACE " alias.*" "" config_rtps_receive_topics "${config_rtps_receive_topics}") + endif() + STRING(REPLACE ", " ";" config_rtps_receive_topics "${config_rtps_receive_topics}") + STRING(REPLACE "\n" "" config_rtps_receive_topics "${config_rtps_receive_topics}") +endif() + +if (FASTRTPSGEN AND (config_rtps_send_topics OR config_rtps_receive_topics)) + option(GENERATE_RTPS_BRIDGE "enable RTPS and microCDR" ON) +endif() + +if (GENERATE_RTPS_BRIDGE) + add_subdirectory(micrortps_client) + + ############################################################################### + # micro-cdr serialization + ############################################################################### + include(px4_git) + px4_add_git_submodule(TARGET git_micro_cdr PATH micro-CDR) + + set(UCDR_SUPERBUILD CACHE BOOL "Disable micro-CDR superbuild compilation.") + add_subdirectory(micro-CDR) + + set(msg_out_path_microcdr ${PX4_BINARY_DIR}/uORB_microcdr/topics) + set(msg_source_out_path_microcdr ${CMAKE_CURRENT_BINARY_DIR}/topics_microcdr_sources) + + set(uorb_headers_microcdr) + set(uorb_sources_microcdr) + + # send topic files + STRING(REGEX REPLACE ";" ", " send_list "${config_rtps_send_topics};${config_rtps_send_alias_topics}") + message(STATUS "RTPS send: ${send_list}") + set(send_topic_files) + foreach(topic ${config_rtps_send_topics}) + list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) + list(APPEND uorb_headers_microcdr ${msg_out_path_microcdr}/${topic}.h) + list(APPEND uorb_sources_microcdr ${msg_source_out_path_microcdr}/${topic}.cpp) + endforeach() + + # receive topic files + STRING(REGEX REPLACE ";" ", " rcv_list "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}") + message(STATUS "RTPS receive: ${rcv_list}") + set(receive_topic_files) + foreach(topic ${config_rtps_receive_topics}) + list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) + list(APPEND uorb_headers_microcdr ${msg_out_path_microcdr}/${topic}.h) + list(APPEND uorb_sources_microcdr ${msg_source_out_path_microcdr}/${topic}.cpp) + endforeach() + + list(REMOVE_DUPLICATES uorb_headers_microcdr) + list(REMOVE_DUPLICATES uorb_sources_microcdr) + + # Generate uORB serialization headers + add_custom_command(OUTPUT ${uorb_headers_microcdr} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py + --headers + -f ${send_topic_files} ${receive_topic_files} + -i ${PX4_SOURCE_DIR}/msg/ + -o ${msg_out_path_microcdr} + -e ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr + -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers_microcdr + -q + DEPENDS + ${receive_topic_files} + ${send_topic_files} + ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py + COMMENT "Generating uORB microcdr topic headers" + VERBATIM + ) + add_custom_target(uorb_headers_microcdr_gen DEPENDS ${uorb_headers_microcdr}) + + # Generate uORB serialization sources + add_custom_command(OUTPUT ${uorb_sources_microcdr} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py + --sources + -f ${send_topic_files} ${receive_topic_files} + -i ${PX4_SOURCE_DIR}/msg/ + -o ${msg_source_out_path_microcdr} + -e ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr + -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources_microcdr + -q + DEPENDS + ${receive_topic_files} + ${send_topic_files} + ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py + COMMENT "Generating uORB microcdr topic sources" + VERBATIM + ) + px4_add_library(uorb_msgs_microcdr ${uorb_sources_microcdr}) + add_dependencies(uorb_msgs_microcdr + uorb_headers_microcdr_gen + git_micro_cdr + microcdr + ) + add_dependencies(microcdr prebuild_targets) + + # microCDR + target_include_directories(uorb_msgs_microcdr + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/micro-CDR/include + ${CMAKE_CURRENT_BINARY_DIR}/micro-CDR/include + ${CMAKE_CURRENT_BINARY_DIR}/micro-CDR/include/microcdr + ) + + target_link_libraries(uorb_msgs_microcdr PRIVATE microcdr) +endif() diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/README.md b/src/prometheus_px4/src/modules/micrortps_bridge/README.md new file mode 100644 index 0000000..22e86f1 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/README.md @@ -0,0 +1 @@ +For see a complete documentation, please follow this [link](https://dev.px4.io/master/en/middleware/micrortps.html) diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.github/dco.yml b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.github/dco.yml new file mode 100644 index 0000000..37e411e --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.github/dco.yml @@ -0,0 +1,2 @@ +require: + members: false \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.github/workflows/codeql-analysis.yml b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.github/workflows/codeql-analysis.yml new file mode 100644 index 0000000..098ed6e --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.github/workflows/codeql-analysis.yml @@ -0,0 +1,67 @@ +# For most projects, this workflow file will not need changing; you simply need +# to commit it to your repository. +# +# You may wish to alter this file to override the set of languages analyzed, +# or to provide custom queries or build logic. +# +# ******** NOTE ******** +# We have attempted to detect the languages in your repository. Please check +# the `language` matrix defined below to confirm you have the correct set of +# supported CodeQL languages. +# +name: "CodeQL" + +on: + push: + branches: [ master, develop ] + pull_request: + # The branches below must be a subset of the branches above + branches: [ master ] + schedule: + - cron: '24 2 * * *' + +jobs: + analyze: + name: Analyze + runs-on: ubuntu-latest + + #strategy: + #fail-fast: false + #matrix: + # language: [ 'c' ] + # CodeQL supports [ 'cpp', 'csharp', 'go', 'java', 'javascript', 'python' ] + # Learn more: + # https://docs.github.com/en/free-pro-team@latest/github/finding-security-vulnerabilities-and-errors-in-your-code/configuring-code-scanning#changing-the-languages-that-are-analyzed + + steps: + - name: Checkout repository + uses: actions/checkout@v2 + + # Initializes the CodeQL tools for scanning. + - name: Initialize CodeQL + uses: github/codeql-action/init@v1 + # with: + # languages: ${{ matrix.language }} + # If you wish to specify custom queries, you can do so here or in a config file. + # By default, queries listed here will override any specified in a config file. + # Prefix the list here with "+" to use these queries and those in the config file. + # queries: ./path/to/local/query, your-org/your-repo/queries@main + + # Autobuild attempts to build any compiled languages (C/C++, C#, or Java). + # If this step fails, then you should remove it and run the build manually (see below) + - name: Autobuild + uses: github/codeql-action/autobuild@v1 + + # ℹ️ Command-line programs to run using the OS shell. + # 📚 https://git.io/JvXDl + + # ✏️ If the Autobuild fails above, remove it and uncomment the following three lines + # and modify them (or add more) to build your code if your project + # uses a compiled language + + #- run: | + # make bootstrap + # make release + + - name: Perform CodeQL Analysis + uses: github/codeql-action/analyze@v1 diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.gitignore b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.gitignore new file mode 100644 index 0000000..272f364 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/.gitignore @@ -0,0 +1,5 @@ +build +examples/performance_cmp_versions +.vscode +.color_coded +.ycm_extra_conf.py diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CMakeLists.txt new file mode 100644 index 0000000..ac8a82f --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CMakeLists.txt @@ -0,0 +1,254 @@ +# Copyright 2019 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +############################################################################### +# CMake build rules for Micro CDR +############################################################################### +cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) + +############################################################################### +# Build options +############################################################################### +option(UCDR_SUPERBUILD "Enable superbuild compilation." ON) +option(UCDR_BUILD_TESTS "Build tests." OFF) +option(UCDR_BUILD_EXAMPLES "Build examples." OFF) +option(UCDR_PIC "Control Position Independent Code." ON) +option(UCDR_ISOLATED_INSTALL "Install the project into a separated folder with version control." ON) +option(BUILD_SHARED_LIBS "Control shared/static library building." OFF) + +option(UCDR_BUILD_CI_TESTS "Build CI test cases." OFF) +if(UCDR_BUILD_CI_TESTS) + set(UCDR_BUILD_TESTS ON) +endif() + +set(CONFIG_BIG_ENDIANNESS OFF CACHE BOOL "Set the machine endianness to big endianness (default: little endianness).") + +# Set CMAKE_BUILD_TYPE to Release by default. +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to 'Release' as none was specified.") + set(CMAKE_BUILD_TYPE Release CACHE STRING + "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." + FORCE) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif() + +############################################################################### +# Product information +############################################################################### +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules) +if(NOT UCDR_SUPERBUILD) + project(microcdr VERSION "1.2.2" LANGUAGES C) +else() + project(ucdr_superbuild NONE) + include(${PROJECT_SOURCE_DIR}/cmake/SuperBuild.cmake) + return() +endif() + +set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} ${PROJECT_BINARY_DIR}/temp_install) + +############################################################################### +# Config +############################################################################### +if(CONFIG_BIG_ENDIANNESS) + set(CONFIG_MACHINE_ENDIANNESS UCDR_BIG_ENDIANNESS) #big +else() + set(CONFIG_MACHINE_ENDIANNESS UCDR_LITTLE_ENDIANNESS) #little +endif() + +############################################################################### +# Check MSVC architecture +############################################################################### +include(${PROJECT_SOURCE_DIR}/cmake/common/check_configuration.cmake) +if(MSVC OR MSVC_IDE) + check_msvc_arch() +endif() + +############################################################################### +# Set install directories +############################################################################### +if(UCDR_ISOLATED_INSTALL) + set(CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}/${PROJECT_NAME}-${PROJECT_VERSION}") +endif() + +include(GNUInstallDirs) +set(BIN_INSTALL_DIR ${CMAKE_INSTALL_BINDIR} CACHE PATH "Installation directory for binaries") +set(INCLUDE_INSTALL_DIR ${CMAKE_INSTALL_INCLUDEDIR} CACHE PATH "Installation directory for C headers") +set(LIB_INSTALL_DIR ${CMAKE_INSTALL_LIBDIR} CACHE PATH "Installation directory for libraries") +set(DATA_INSTALL_DIR ${CMAKE_INSTALL_DATADIR} CACHE PATH "Installation directory for data") +if(WIN32) + set(LICENSE_INSTALL_DIR . CACHE PATH "Installation directory for licenses") +else() + set(LICENSE_INSTALL_DIR ${DATA_INSTALL_DIR}/${PROJECT_NAME} CACHE PATH "Installation directory for licenses") +endif() + +############################################################################### +# Targets +############################################################################### +# Library +add_library(${PROJECT_NAME} + src/c/common.c + src/c/types/basic.c + src/c/types/string.c + src/c/types/array.c + src/c/types/sequence.c + ) + +# Set common properties +set_common_compile_options(${PROJECT_NAME}) +set_target_properties(${PROJECT_NAME} PROPERTIES + VERSION + ${PROJECT_VERSION} + SOVERSION + ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + C_STANDARD + 99 + C_STANDARD_REQUIRED + YES + POSITION_INDEPENDENT_CODE + ${UCDR_PIC} + ) + +target_compile_options(${PROJECT_NAME} + PRIVATE + $<$:-fdata-sections -ffunction-sections> + ) + +# Set Windows shared libraries name +get_target_property(UCDR_TARGET_TYPE ${PROJECT_NAME} TYPE) +if((CMAKE_SYSTEM_NAME STREQUAL "Windows") AND (UCDR_TARGET_TYPE STREQUAL "SHARED_LIBRARY")) + set_target_properties(${PROJECT_NAME} PROPERTIES + RELEASE_POSTFIX + -${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + RELWITHDEBINFO_POSTFIX + -${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + DEBUG_POSTFIX + d-${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR} + ) + + target_compile_definitions(${PROJECT_NAME} + PUBLIC + ${PROJECT_NAME}_SHARED + ) +endif() + +# Define public headers +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + $ + ) + +############################################################################### +# Examples +############################################################################### +if(UCDR_BUILD_EXAMPLES) + add_subdirectory(examples/basic) + add_subdirectory(examples/fragmentation) +endif() + +############################################################################### +# Testing +############################################################################### +if(UCDR_BUILD_CI_TESTS) + include(CTest) + add_subdirectory(test/case) +endif() + +if(UCDR_BUILD_TESTS) + include(${PROJECT_SOURCE_DIR}/cmake/common/gtest.cmake) + enable_testing() + add_subdirectory(test) +endif() + +############################################################################### +# Packaging +############################################################################### +# Install library +install( + TARGETS + ${PROJECT_NAME} + EXPORT + ${PROJECT_NAME}Targets + ARCHIVE DESTINATION + ${LIB_INSTALL_DIR} + LIBRARY DESTINATION + ${LIB_INSTALL_DIR} + RUNTIME DESTINATION + ${BIN_INSTALL_DIR} + COMPONENT + libraries + ) + +# Install includes +install( + DIRECTORY + ${PROJECT_SOURCE_DIR}/include/ucdr/ + DESTINATION + ${INCLUDE_INSTALL_DIR}/ucdr + FILES_MATCHING + PATTERN "*.h" + ) + +# Generate config.h +configure_file(${PROJECT_SOURCE_DIR}/include/ucdr/config.h.in + ${PROJECT_BINARY_DIR}/include/ucdr/config.h + ) + +# Install config.h +install( + FILES + ${PROJECT_BINARY_DIR}/include/ucdr/config.h + DESTINATION + ${INCLUDE_INSTALL_DIR}/ucdr + ) + +# Export library +install( + EXPORT + ${PROJECT_NAME}Targets + DESTINATION + ${DATA_INSTALL_DIR}/${PROJECT_NAME}/cmake + ) + +# Package configuration +include(CMakePackageConfigHelpers) + +configure_package_config_file( + ${PROJECT_SOURCE_DIR}/cmake/packaging/Config.cmake.in + ${PROJECT_BINARY_DIR}/cmake/config/${PROJECT_NAME}Config.cmake + INSTALL_DESTINATION + ${DATA_INSTALL_DIR}/${PROJECT_NAME}/cmake + PATH_VARS + BIN_INSTALL_DIR + INCLUDE_INSTALL_DIR + LIB_INSTALL_DIR + DATA_INSTALL_DIR + ) + +write_basic_package_version_file( + ${PROJECT_BINARY_DIR}/cmake/config/${PROJECT_NAME}ConfigVersion.cmake + VERSION + ${PROJECT_VERSION} + COMPATIBILITY + SameMajorVersion + ) + +install( + FILES + ${PROJECT_BINARY_DIR}/cmake/config/${PROJECT_NAME}Config.cmake + ${PROJECT_BINARY_DIR}/cmake/config/${PROJECT_NAME}ConfigVersion.cmake + DESTINATION + ${DATA_INSTALL_DIR}/${PROJECT_NAME}/cmake + ) diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CONTRIBUTING.md b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CONTRIBUTING.md new file mode 100644 index 0000000..a527c05 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CONTRIBUTING.md @@ -0,0 +1,36 @@ +# Contribution Guidelines + +The present document provides a set of guidelines to which contributors must adhere. + +- [Contribution Guidelines](#contribution-guidelines) + - [Contributions Licensing](#contributions-licensing) + - [Developer Certificate of Origin](#developer-certificate-of-origin) + - [Code Coverage](#code-coverage) + - [General considerations](#general-considerations) + +## Contributions Licensing + +Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that [license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +## Developer Certificate of Origin + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` line to commit messages to certify that they have the right to submit the code they are contributing to the project according to the [Developer Certificate of Origin (DCO)](https://developercertificate.org/). + +## Code Coverage + +As stated in [QUALITY.md](QUALITY.md), all contributions to the project must increase line code coverage. +Because of this, contributors are asked to locally run a coverage assessment that ensures that code coverage has increased when compared to the latest execution of the [nightly coverage CI job](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Linux/511/coverage/). + +## General considerations + +Any other contributing policy can be found in [eProsima general guidelines](https://github.com/eProsima/policies/blob/main/CONTRIBUTING.md). diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CTestConfig.cmake b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CTestConfig.cmake new file mode 100644 index 0000000..f10eab5 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/CTestConfig.cmake @@ -0,0 +1,22 @@ +# Copyright 2019 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +if(CMAKE_SYSTEM_NAME STREQUAL "Linux") + # MemoryCheck configuration. + find_program(MEMORYCHECK_COMMAND NAMES valgrind) + set(MEMORYCHECK_COMMAND_OPTIONS "${MEMORYCHECK_COMMAND_OPTIONS} --quiet --tool=memcheck --leak-check=yes --show-reachable=yes --num-callers=50 --xml=yes --xml-file=test_%p_memcheck.xml \"--suppressions=${CMAKE_CURRENT_SOURCE_DIR}/ci/valgrind.supp\"") + + # Coverage configuration. + find_program(COVERAGE_COMMAND NAMES gcov) +endif() \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/LICENSE b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/PLATFORM_SUPPORT.md b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/PLATFORM_SUPPORT.md new file mode 100644 index 0000000..867947d --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/PLATFORM_SUPPORT.md @@ -0,0 +1,47 @@ +# Platform Support + +This document reflects the level of support offered by **eProsima Micro CDR** on different platforms as per the following +definitions: + +## Tier 1 + +Tier 1 platforms are subjected to our unit test suite and other testing tools on a frequent basis including continuous +integration jobs and nightly jobs. +Errors or bugs discovered in these platforms are prioritized for correction by the development team. +Significant errors discovered in Tier 1 platforms can impact release dates and we strive to resolve all known high +priority errors in Tier 1 platforms prior to new version releases. + +## Tier 2 + +Tier 2 platforms are subject to periodic CI testing which runs both builds and tests with publicly accessible results. +The CI is expected to be run at least within a week from the last relevant changes for the current release of **Micro CDR**. +Installation instructions should be available and up-to-date in order for a platform to be listed in this category. +Package-level binary packages may not be provided but providing a downloadable archive of the built workspace is +encouraged. +Errors may be present in released product versions for Tier 2 platforms. +Known errors in Tier 2 platforms will be addressed subject to resource availability on a best effort basis and may or +may not be corrected prior to new version releases. +One or more entities should be committed to continuing support of the platform. + +## Tier 3 + +Tier 3 platforms are those for which community reports indicate that the release is functional. +The development team does not run the unit test suite nor perform any other tests on platforms in Tier 3. +Community members may provide assistance with these platforms. + +## Platforms + +| Architecture | Ubuntu Focal (20.04) | Windows 10 (VS2019) | Debian Buster (10) | +| ------------ | -------------------- | ------------------- | ------------------ | +| amd64 | Tier 1 [s] | Tier 1 [s] | Tier 3 [s] | +| amd32 | | Tier 1 [s] | | +| arm64 | Tier 3 [s] | | Tier 3 [s] | +| arm32 | Tier 3 [s] | | Tier 3 [s] | + +" [s] " Compilation from source. + +Other Tier 3 OS: + +* FreeRTOS +* Zephyr RTOS +* Nuttx diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/QUALITY.md b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/QUALITY.md new file mode 100644 index 0000000..b8e21c4 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/QUALITY.md @@ -0,0 +1,193 @@ +This document is a declaration of software quality for **eProsima Micro CDR** based on the guidelines provided in the [ROS 2 REP-2004 document](https://www.ros.org/reps/rep-2004.html). + +# Quality Declaration + +**eProsima Micro CDR** is a C99 library that functions as a serialization tool, according to the [standard CDR](https://www.omg.org/cgi-bin/doc?formal/02-06-51) serialization mechanism. + +**eProsima Micro CDR** claims to be in the **Quality Level 1** category. + +Below are the rationales, notes and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html#package-requirements). + +## Version Policy [1] + +### Version Scheme [1.i] + +The **Versioning Policy Declaration** for **eProsima Micro CDR** can be found [here](VERSIONING.md) and it adheres to [`semver`](https://semver.org/). + +### Version Stability [1.ii] + +**eProsima Micro CDR** is at a stable version, i.e. `>=1.0.0`. +The latest version and the release notes can be found [here](https://github.com/eProsima/Micro-CDR/releases). + +### Public API Declaration [1.iii] + +The public API is documented [in this repo README.md](https://github.com/eProsima/Micro-CDR#api-functions). + +### API Stability Policy [1.iv] + +**eProsima Micro CDR** will only break public API between major releases. + +### ABI Stability Policy [1.v] + +Any ABI break in **eProsima Micro CDR** will be done between minor versions and it should be clearly stated in the release notes. + +## Change Control Process [2] + +The stability of **eProsima Micro CDR** is ensured through reviews, CI and tests. +The change control process can be found in [CONTRIBUTING](CONTRIBUTING.md) + +All changes to **eProsima Micro CDR** occur through pull requests that are required to pass all CI tests. +In case of failure, only maintainers can merge the pull request, and only when there is enough evidence that the failure is unrelated to the change. +Additionally, all pull requests must have at least one positive review from another contributor that did not author the pull request. + +### Change Requests [2.i] + +All changes will occur through a pull request. + +### Contributor Origin [2.ii] + +**eProsima Micro CDR** uses the [Developer Certificate of Origin (DCO)](https://developercertificate.org/) as its confirmation of contributor origin policy since version 1.2.2. +More information can be found in [CONTRIBUTING](CONTRIBUTING.md) + +### Peer Review Policy [2.iii] + +All pull requests will be peer-reviewed by at least one other contributor who did not author the pull request. Approval is required before merging. + +### Continuous Integration [2.iv] + +All pull requests must pass CI to be considered for merging, unless maintainers consider that there is enough evidence that the failure is unrelated to the changes. +CI testing is automatically triggered by incoming pull requests. +Current nightly results for all supported platforms can be checked at the links: + +* Linux [![Linux ci](http://jenkins.eprosima.com:8080/view/Nightly/job/Micro-CDR%20Nightly%20Master%20Linux/badge/icon?subject=%20%20%20Linux%20CI%20)](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Linux/) +* Windows [![Windows ci](http://jenkins.eprosima.com:8080/job/Micro-CDR%20Nightly%20Master%20Windows/badge/icon?subject=%20%20%20%20Windows%20CI%20)](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Windows/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging as stated in [CONTRIBUTING](CONTRIBUTING.md). + +## Documentation [3] + +### Feature Documentation [3.i] + +**eProsima Micro CDR** provides one feature corresponding to its serialization mechanisms: + +* [Standard CDR](https://www.omg.org/cgi-bin/doc?formal/02-06-51) serialization mechanism. + +### Public API Documentation [3.ii] + +**eProsima Micro CDR** includes a complete API Reference documented [in this repo README.md](https://github.com/eProsima/Micro-CDR#api-functions). + +### License [3.iii] + +The license for **eProsima Micro CDR** is Apache 2.0, and a summary can be found in each source file. +A full copy of the license can be found [here](LICENSE). + +### Copyright Statements [3.iv] + +The **eProsima Micro CDR** copyright holder provides a statement of copyright in each source file. + +## Testing [4] + +### Feature Testing [4.i] + +**eProsima Micro CDR** provides tests which simulate typical usage, and they are located in the [`test` directory](test). +New features are required to have tests before being added as stated in [CONTRIBUTING](CONTRIBUTING.md). +Current nightly results can be found here: + +* Linux [![Linux ci](http://jenkins.eprosima.com:8080/view/Nightly/job/Micro-CDR%20Nightly%20Master%20Linux/badge/icon?subject=%20%20%20Linux%20CI%20)](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Linux/) +* Windows [![Windows ci](http://jenkins.eprosima.com:8080/job/Micro-CDR%20Nightly%20Master%20Windows/badge/icon?subject=%20%20%20%20Windows%20CI%20)](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Windows/) + +### Public API Testing [4.ii] + +Each part of the public API has tests, and new additions or changes to the public API require tests before being added. +The tests aim to cover typical usage and corner cases. + +### Coverage [4.iii] + +[![Coverage](https://img.shields.io/jenkins/coverage/cobertura?jobUrl=http%3A%2F%2Fjenkins.eprosima.com%3A8080%2Fview%2FMicro%2520XRCE%2Fjob%2FMicro-CDR%2520Nightly%2520Master%2520Linux%2F)](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Linux/) +*eProsima Micro CDR* aims to provide a line coverage **above 90%**. +*Micro CDR* code coverage policy comprises: +1. All contributions to *Micro CDR* must increase (or at least keep) the current line coverage. + This is done to ensure that the **90%** line coverage goal is eventually met. +1. Line coverage regressions are only permitted if properly justified and accepted by maintainers. +1. If the CI system reports a coverage regression after a pull request has been merged, the maintainers must study the case and decide how to proceed, most often reverting the changes and asking for a more thorough testing of the committed changes. +2. This policy is enforced through the [nightly Micro CDR CI job](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Linux/). + +As stated in [CONTRIBUTING.md](CONTRIBUTING.md), developers and contributors are requested to run a line coverage assessment locally before submitting a PR. + +### Performance [4.iv] + +**eProsima Micro CDR** does not provide performance tests. +However, performance is indirectly tested by *eprosima Micro XRCE-DDS Client*. + +Any performance regression detected in *eProsima Micro XRCE-DDS Client* would be analyz¡sed and, in case it is related to **eProsima Micro CDR** or could be solved by modifying this library, changes can be made to the library in order to revert the performance regression. + +### Linters and Static Analysis [4.v] + +**eProsima Micro CDR** [code style](https://github.com/eProsima/cpp-style) is enforced using [*uncrustify*](https://github.com/uncrustify/uncrustify). +Among the CI tests, there are tests that ensure that every pull request is compliant with the code style. +The latest pull request results can be seen [here](http://jenkins.eprosima.com:8080/job/Micro-CDR%20Manual%20Linux/lastBuild/). + +**eProsima Micro CDR** uses [Synopsis Coverity static code analysis](https://scan.coverity.com/projects/eprosima-micro-cdr). + +**eProsima Micro CDR** uses [CodeQL](https://github.com/eProsima/Micro-CDR/security/code-scanning?query=tool%3ACodeQL) to find security issues on the code. + +## Dependencies [5] + +### Direct Runtime Dependencies [5.iii] + +**eProsima Micro CDR** has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +**eProsima Micro CDR** supports the following platforms and tests each change against all of them as can be seen in the current nightly results: + +* Linux [![Linux ci](http://jenkins.eprosima.com:8080/view/Nightly/job/Micro-CDR%20Nightly%20Master%20Linux/badge/icon?subject=%20%20%20Linux%20CI%20)](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Linux/) +* Windows [![Windows ci](http://jenkins.eprosima.com:8080/job/Micro-CDR%20Nightly%20Master%20Windows/badge/icon?subject=%20%20%20%20Windows%20CI%20)](http://jenkins.eprosima.com:8080/view/Micro%20XRCE/job/Micro-CDR%20Nightly%20Master%20Windows/) + +More information about the supported platforms can be found in [PLATFORM_SUPPORT](PLATFORM_SUPPORT.md) + +## Vulnerability Disclosure Policy [7.i] + +**eprosima Micro CDR** vulnerability Disclosure Policy can be found [here](https://github.com/eProsima/policies/blob/main/VULNERABILITY.md) + +# Current Status Summary + +The chart below compares the requirements in the [REP-2004](https://www.ros.org/reps/rep-2004.html#quality-level-comparison-chart) with the current state of **eprosima Micro CDR** +| Number | Requirement | Current State | +| ------- | ------------------------------------------------- | ------------- | +| 1 | **Version policy** | --- | +| 1.i | Version Policy available | ✓ | +| 1.ii | Stable version | ✓ | +| 1.iii | Declared public API | ✓ | +| 1.iv | API stability policy | ✓ | +| 1.v | ABI stability policy | ✓ | +| 2 | **Change control process** | --- | +| 2.i | All changes occur on change request | ✓ | +| 2.ii | Contributor origin (DCO, CLA, etc) | ✓ | +| 2.iii | Peer review policy | ✓ | +| 2.iv | CI policy for change requests | ✓ | +| 2.v | Documentation policy for change requests | ✓ | +| 3 | **Documentation** | --- | +| 3.i | Per feature documentation | ✓ | +| 3.ii | Per public API item documentation | ✓ | +| 3.iii | Declared License(s) | ✓ | +| 3.iv | Copyright in source files | ✓ | +| 3.v.a | Quality declaration linked to README | ✓ | +| 3.v.b | Centralized declaration available for peer review | ✓ | +| 4 | **Testing** | --- | +| 4.i | Feature items tests | ✓ | +| 4.ii | Public API tests | ✓ | +| 4.iii.a | Using coverage | ✓ | +| 4.iii.b | Coverage policy | ✓ | +| 4.iv.a | Performance tests (if applicable) | N/A | +| 4.iv.b | Performance tests policy | N/A | +| 4.v.a | Code style enforcement (linters) | ✓ | +| 4.v.b | Use of static analysis tools | ✓ | +| 5 | **Dependencies** | --- | +| 5.iii | Justifies quality use of dependencies | ✓ | +| 6 | **Platform support** | --- | +| 6.i | Support targets Tier1 ROS platforms | ✓ | +| 7 | **Security** | --- | +| 7.i | Vulnerability Disclosure Policy | ✓ | diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/README.md b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/README.md new file mode 100644 index 0000000..0f7b1f2 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/README.md @@ -0,0 +1,308 @@ +# eProsima Micro CDR + +[![Releases](https://img.shields.io/github/release/eProsima/Micro-CDR.svg)](https://github.com/eProsima/Micro-CDR/releases) +[![License](https://img.shields.io/github/license/eProsima/Micro-CDR.svg)](https://github.com/eProsima/Micro-CDR/blob/master/LICENSE) +[![Issues](https://img.shields.io/github/issues/eProsima/Micro-CDR.svg)](https://github.com/eProsima/Micro-CDR/issues) +[![Forks](https://img.shields.io/github/forks/eProsima/Micro-CDR.svg)](https://github.com/eProsima/Micro-CDR/network/members) +[![Stars](https://img.shields.io/github/stars/eProsima/Micro-CDR.svg)](https://github.com/eProsima/Micro-CDR/stargazers) + + + +*eProsima Micro CDR* is a *C* library implementing the *CDR* standard serialization methods. +This library is focused on embedded and resource-limited systems. + +*Micro CDR* uses a static buffer, and allow to serialize and deserialize in both, big endianness and little endianness. + +## Usage examples +This is a code example showing the serialization and deserialization of a string. +As *Micro CDR* uses a static buffer, the user has to provide a defined buffer and its size during the *ucdrBuffer* creation. + +```c + #include + #include + + #define BUFFER_LENGTH 256 + + int main(int argc, char** args) + { + // Data buffer + uint8_t buffer[BUFFER_LENGTH]; + + // Structs for handle the buffer. + ucdrBuffer writer; + ucdrBuffer reader; + + // Initialize the MicroBuffers for working with an user-managed buffer. + ucdr_init_buffer(&writer, buffer, BUFFER_LENGTH); + ucdr_init_buffer(&reader, buffer, BUFFER_LENGTH); + + // Serialize data + char input[16] = "Hello Micro CDR!"; //16 characters + ucdr_serialize_array_char(&writer, input, 16); + + // Deserialize data + char output[16]; + ucdr_deserialize_array_char(&reader, output, 16); + + printf("Input: %s\n", input); + printf("Output: %s\n", output); + + return 0; + } +``` + +## API functions + +```c +void ucdr_init_buffer (ucdrBuffer* ub, uint8_t* data, size_t size); +void ucdr_init_buffer_origin (ucdrBuffer* ub, uint8_t* data, size_t size, size_t origin); +void ucdr_init_buffer_origin_offset (ucdrBuffer* ub, uint8_t* data, size_t size, size_t origin, size_t offset); +void ucdr_init_buffer_origin_offset_endian (ucdrBuffer* ub, uint8_t* data, size_t size, size_t origin, size_t offset, ucdrEndianness endianness); +``` +Initialize a `ucdrBuffer` structure, the main struct of *Micro CDR*. +- `ub`: the `ucdrBuffer` struct. +- `data`: the buffer that the `ucdrBuffer` will use. +- `size`: the size of the buffer that the `ucdrBuffer` will use. +- `origin`: the origin of the XCDR stream. +- `offset`: where the serialization/deserialization will start. +- `endianness`: the endianness of the XCDR stream. +Initially, the serialization/deserialization starts at the beginning of the buffer. + +--- + +```c +void ucdr_copy_buffer (ucdrBuffer* ub_dest, const ucdrBuffer* ub_source); +``` +Copy a `ucdrBuffer` structure data to another `ucdrBuffer` structure. +- `ub_dest`: the destination `ucdrBuffer` struct. +- `ub_source`: the origin initialized `ucdrBuffer` struct. + +--- + +```c +void ucdr_set_on_full_buffer_callback (ucdrBuffer* ub, OnFullBuffer on_full_buffer, void* args); +``` +Sets the `on_full_buffer` callback which will be called each time the buffer arises its end. +- `ub`: the `ucdrBuffer` struct. +- `on_full_buffer`: the callcack. +- `args`: the argument passes to the callback. + +--- + +```c +void ucdr_reset_buffer (ucdrBuffer* ub); +void ucdr_reset_buffer_offset(ucdrBuffer* ub, size_t offset); +``` +Reset the `ucdrBuffer` as the same state that it was created. +- `ub`: the `ucdrBuffer` struct. +- `offset`: where the serialization/deserialization will start. +Initially, the serialization/deserialization starts at the beginning of the buffer. + +--- + +```c +void ucdr_align_to (ucdrBuffer* ub, size_t size); +``` +Align the ucdrBuffer to the size `size`. +After call this function, the serialization pointer will be moved only if the current `ucdrBuffer` was not alignment to the passed value. + +- `ub`: the `ucdrBuffer` struct +- `size`: the target size alignment. + +--- + +```c +size_t ucdr_alignment(size_t buffer_position, size_t data_size); +``` +Returns the alignment necessary to serialize/deserialize a type with `data_size` size. + +- `buffer_position`: the current serialization/deserialization position of the `ucdrBuffer`. (Typically `ub->iterator - ub->init`). +- `data_size`: the bytes of the data that you are asking for. + +--- + +```c +size_t ucdr_buffer_alignment(const ucdrBuffer* ub, size_t data_size); +``` +Returns the alignment necessary to serialize/deserialize a type with `data_size` size into the `ucdrBuffer` given. + +- `ub`: the `ucdrBuffer` struct to ask the alignment. +- `data_size`: the bytes of the data that you are asking for. +--- + +```c +void ucdr_advance_buffer(const ucdrBuffer* ub, size_t size); +``` +Advances the XCDR stream `size` bytes without de/serialization involved. + +- `ub`: the `ucdrBuffer` struct to ask the alignment. +- `size`: the bytes to advance. +--- + +```c +size_t ucdr_buffer_size(const ucdrBuffer* ub); +``` +Returns the memory size of the buffer. +- `ub`: the `ucdrBuffer` struct + +--- + +```c +size_t ucdr_buffer_length(const ucdrBuffer* ub); +``` +Returns the size of the serialized/deserialized data. +- `ub`: the `ucdrBuffer` struct + +--- + +```c +size_t ucdr_buffer_remaining(const ucdrBuffer* ub); +``` +Returns the remaining size for the serializing/deserializing. +- `ub`: the `ucdrBuffer` struct + +--- + +```c +ucdrEndianness ucdr_buffer_endianness(const ucdrBuffer* ub); +``` +Returns the serialization/deserialization endianness. +- `ub`: the `ucdrBuffer` struct + +--- + +```c +bool ucdr_buffer_error(const ucdrBuffer* ub); +``` +Returns the status error of the `ucdrBuffer`. +- `ub`: the `ucdrBuffer` struct + +### Serialization/deserialization functions +Adding to this, there is a big set of functions for deserialize and deserialize different kind of types: +- Basics: `bool`, `char`, `int8_t`, `uint8_t`,`int16_t`, `uint16_t`,`int32_t`, `uint32_t`,`int64_t`, `uint64_t`,`float`, `double`. +- Arrays: Any fixed size of basics types. +- Sequence: Similar to arrays, but the information about the size is serialized along with the data. +- String: Wrapper of char sequence. + +### Endianness +*Micro CDR* supports little and big endianness. +The **machine endianness** can be set by the cmake variable: `CONFIG_BIG_ENDIANNESS`. +By default, if this varible is `OFF` which means that the machine endianness is little endianness. + +The `ucdrBuffer` endianness can be set by the `endianness` parameter of the structure to `UCDR_BIG_ENDIANNESS` or `UCDR_LITTLE_ENDIANNESS`. +Also, there are a functions that allow to force an endianness independiently of the `ucdrBuffer` endianness in their serialization/deserialization. +These functions contains the name `endianness` in their signature. + +### Error +All serialization/deserialization functions return a boolean indicating the result of their operations. +When a serialization/deserialization could not be possible (the type can not be serialized, or the capacity of the destination buffer is not enough), +an status error is setted into the `ucdrBuffer`. +If a `ucdrBuffer` has an error state, the next serialization/deserialization operations will not works and will return `false` in their execution. +A buffer marked with an error can be used, but any serialization/deserialization operation over it will not produce any effect. + +If is kwown that an operation can fails over a `ucdrBuffer`, and its necessary to continue with the serialization/deserialization if it happens, +the `ucdrBuffer` state can be saved using the `ucdr_copy_buffer` function. +After the application of the wrong serialization/deserialization, only the `ucdrBuffer` that performed the operation will have a dirty state. + +## Serialization/deserialization list +The available modes of serialization/deserializations in *Micro CDR* are shown in the following table. + +| Type | Endianness | +| -------------------- | ---------- | +| bool | | +| char | | +| int8 | | +| uint8 | | +| int16 | | +| int16 | endianness | +| uint16 | | +| uint16 | endianness | +| int32 | | +| int32 | endianness | +| uint32 | | +| uint32 | endianness | +| int64 | | +| int64 | endianness | +| uint64 | | +| uint64 | endianness | +| float | | +| float | endianness | +| double | | +| double | endianness | +| string | | +| string | endianness | +| bool array | | +| char array | | +| int8 array | | +| uint8 array | | +| int16 array | | +| int16 array | endianness | +| uint16 array | | +| uint16 array | endianness | +| int32 array | | +| int32 array | endianness | +| uint32 array | | +| uint32 array | endianness | +| int64 array | | +| int64 array | endianness | +| uint64 array | | +| uint64 array | endianness | +| float array | | +| float array | endianness | +| double array | | +| double array | endianness | +| bool sequence | | +| bool sequence | endianness | +| char sequence | | +| char sequence | endianness | +| int8 sequence | | +| int8 sequence | endianness | +| uint8 sequence | | +| uint8 sequence | endianness | +| int16 sequence | | +| int16 sequence | endianness | +| uint16 sequence | | +| uint16 sequence | endianness | +| int32 sequence | | +| int32 sequence | endianness | +| uint32 sequence | | +| uint32 sequence | endianness | +| int64 sequence | | +| int64 sequence | endianness | +| uint64 sequence | | +| uint64 sequence | endianness | +| float sequence | | +| float sequence | endianness | +| double sequence | | +| double sequence | endianness | + +## Additional features +### Endianness +*Micro CDR* supports little and big endianness. +The configuration can be done by cmake with the cmake `__BIG_ENDIAN__` variable. +A `0` value implies that the serialization will performed into a little endian machine, and `1` into a big endian machine. + +The default endianness serialization can be choosen by setting the `endianness` parameter of a `ucdrBuffer` to `UCDR_BIG_ENDIANNESS` or `UCDR_LITTLE_ENDIANNESS`. +Also, there are a functions that allow to force an endianness in their serialization/deserialization. +These functions contains the name `endiannness` in their signature. + +### Error +All serialization/deserialization functions return a boolean indicating the result of their operations. +When a serialization/deserialization could not be possible (the type can not be serialized, or the capacity of the destination buffer is not enough), +an status error is setted into the `ucdrBuffer`. +If a `ucdrBuffer` has an error state, the next serialization/deserialization operations will not works and will return `false` in their execution. +A buffer marked with an error can be used, but any serialization/deserialization operation over it will not produce any effect. + +If is kwown that an operation can fails over a `ucdrBuffer`, and its necessary to continue with the serialization/deserialization if it happens, +the `ucdrBuffer` state can be saved using the `ucdr_copy_buffer` function. +After the application of the wrong serialization/deserialization, only the `ucdrBuffer` that performed the operation will have a dirty state. + +### Full buffer callback +Micro CDR provides a callback that the user can set in order to control the behavior when the `ucdrBuffer` can not serialize/deserialize anymore because the buffer is full. +This allows to create a better management error and/or modify the buffer location of the `ucdrBuffer`. +The last possibility gives the user the capacity to use several small buffers for a big serialization (see the *fragmentation* example). + +## Quality Declaration + +**eProsima Micro CDR** claims to be in the **Quality Level 1** category based on the guidelines provided by [ROS 2](https://ros.org/reps/rep-2004.html). +See the [Quality Declaration](QUALITY.md) for more details. diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/VERSIONING.md b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/VERSIONING.md new file mode 100644 index 0000000..d7ee0e1 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/VERSIONING.md @@ -0,0 +1,9 @@ +Versioning policy declaration +============================= + +Starting on v1.0.0, the version numbers for this library will adhere to the versioning policies defined by [Semantic Versioning](https://semver.org/). + +This means that API breaks should only happen between major version changes. +If an ABI break is required, it should be done between minor versions, and it should be clearly stated in the release notes. + +The changes included in each version can be found in the [release notes](https://github.com/eProsima/Micro-CDR/releases) diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/CTestJenkins.cmake b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/CTestJenkins.cmake new file mode 100644 index 0000000..1c5bbfb --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/CTestJenkins.cmake @@ -0,0 +1,88 @@ +# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set(CTEST_SOURCE_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") +set(CTEST_BINARY_DIRECTORY "${CTEST_SOURCE_DIRECTORY}/build") +set(CTEST_TEST_TIMEOUT 300) + +site_name(CTEST_SITE) +set(CTEST_BUILD_NAME "${JENKINS_BUILD_NAME}") +set(CTEST_BUILD_OPTIONS "${JENKINS_BUILD_OPTIONS}") +set(CTEST_BUILD_CONFIGURATION "${JENKINS_BUILD_CONFIGURATION}") + +set(CTEST_MEMORYCHECK_COMMAND_OPTIONS "${CTEST_MEMORYCHECK_COMMAND_OPTIONS} --quiet --tool=memcheck --leak-check=yes --show-reachable=yes --num-callers=50 --xml=yes --xml-file=test_%p_memcheck.xml \"--suppressions=${CTEST_SOURCE_DIRECTORY}/valgrind.supp\"") + +set(CTEST_COVERAGE_C_FLAGS "-DCMAKE_C_FLAGS:STRING=-fwrapv -fprofile-arcs -ftest-coverage") +set(CTEST_COVERAGE_CXX_FLAGS "-DCMAKE_CXX_FLAGS:STRING=-fwrapv -fprofile-arcs -ftest-coverage") +set(CTEST_COVERAGE_EXE_LD_FLAGS "-DCMAKE_EXE_LINKER_FLAGS:STRING=-fprofile-arcs -ftest-coverage") +set(CTEST_COVERAGE_SHARED_LD_FLAGS "-DCMAKE_SHARED_LINKER_FLAGS:STRING=-fprofile-arcs -ftest-coverage") + +include(ProcessorCount) +ProcessorCount(NUMBER_PROCESSORS) +message("Number of processors: " ${NUMBER_PROCESSORS}) +if(NOT NUMBER_PROCESSORS EQUAL 0) + if(${JENKINS_GENERATOR} MATCHES "Unix Makefiles") + set(CTEST_BUILD_FLAGS "-j${NUMBER_PROCESSORS} -l${NUMBER_PROCESSORS}") + # Error using visual studio with multicore + #elseif(${JENKINS_GENERATOR} MATCHES "Visual Studio") + #set(CTEST_WIN_CXX_FLAGS "-DEPROSIMA_EXTRA_CMAKE_CXX_FLAGS:STRING=/MP") + endif() + set(CTEST_TEST_ARGS ${CTEST_TEST_ARGS} PARALLEL_LEVEL ${NUMBER_PROCESSORS}) +endif() + +# Check CMake version for QUIET parameter +if(${CMAKE_MAJOR_VERSION} GREATER 3 OR (${CMAKE_MAJOR_VERSION} EQUAL 3 AND ${CMAKE_MINOR_VERSION} GREATER 2)) + set(QUIET_ QUIET) +endif() + +ctest_empty_binary_directory(${CTEST_BINARY_DIRECTORY}) + +if(UNIX) + find_program(CTEST_COVERAGE_COMMAND NAMES gcov) + find_program(CTEST_MEMORYCHECK_COMMAND NAMES valgrind) +endif() + +set(CTEST_CONFIGURE_COMMAND "${CMAKE_COMMAND}") +set(CTEST_CONFIGURE_COMMAND "${CTEST_CONFIGURE_COMMAND} -DCMAKE_BUILD_TYPE=${CTEST_BUILD_CONFIGURATION}") +set(CTEST_CONFIGURE_COMMAND "${CTEST_CONFIGURE_COMMAND} ${CTEST_BUILD_OPTIONS}") +set(CTEST_CONFIGURE_COMMAND "${CTEST_CONFIGURE_COMMAND} ${CTEST_WIN_CXX_FLAGS}") +if(CTEST_COVERAGE_COMMAND AND NOT DISABLE_CTEST_COVERAGE) + set(CTEST_CONFIGURE_COMMAND "${CTEST_CONFIGURE_COMMAND} \"${CTEST_COVERAGE_C_FLAGS}\"") + set(CTEST_CONFIGURE_COMMAND "${CTEST_CONFIGURE_COMMAND} \"${CTEST_COVERAGE_CXX_FLAGS}\"") + set(CTEST_CONFIGURE_COMMAND "${CTEST_CONFIGURE_COMMAND} \"${CTEST_COVERAGE_EXE_LD_FLAGS}\"") + set(CTEST_CONFIGURE_COMMAND "${CTEST_CONFIGURE_COMMAND} \"${CTEST_COVERAGE_SHARED_LD_FLAGS}\"") +endif() +set(CTEST_CONFIGURE_COMMAND "${CTEST_CONFIGURE_COMMAND} \"${CTEST_SOURCE_DIRECTORY}\"") +set(CTEST_BUILD_COMMAND "${CMAKE_COMMAND} --build .") +if(WIN32) + set(CTEST_BUILD_COMMAND "${CTEST_BUILD_COMMAND} --config ${CTEST_BUILD_CONFIGURATION}") +endif() +set(CTEST_CONFIGURATION_TYPE ${CTEST_BUILD_CONFIGURATION}) + +ctest_start("${JENKINS_DASHBOARD}" ${QUIET_}) +ctest_configure(RETURN_VALUE CONFIGURING_RET_VALUE ${QUIET_}) +ctest_build(RETURN_VALUE BUILDING_RET_VALUE ${QUIET_}) +ctest_test(${QUIET_}) +if(CTEST_COVERAGE_COMMAND AND NOT DISABLE_CTEST_COVERAGE) + ctest_coverage(${QUIET_}) +endif() +if(CTEST_MEMORYCHECK_COMMAND AND NOT DISABLE_CTEST_MEMORYCHECK) + ctest_memcheck(EXCLUDE_LABEL NoMemoryCheck ${QUIET_}) +endif() + +if(NOT CONFIGURING_RET_VALUE AND NOT BUILDING_RET_VALUE) + message(0) +else() + message(255) +endif() diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/linux/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/linux/CMakeLists.txt new file mode 100644 index 0000000..863f4a2 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/linux/CMakeLists.txt @@ -0,0 +1,90 @@ +# Copyright 2019 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) + +project(microcdr_ci LANGUAGES C CXX) + +include(ExternalProject) +include(CheckCXXCompilerFlag) +include(CheckCCompilerFlag) + +set(_c_flags "-fwrapv -fprofile-arcs -ftest-coverage --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") +set(_cxx_flags "-fwrapv -fprofile-arcs -ftest-coverage --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") +set(_exe_linker_flags "-fprofile-arcs -ftest-coverage --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") +set(_shared_linker_flags "-fprofile-arcs -ftest-coverage --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") + +check_cxx_compiler_flag("-fprofile-abs-path" _have_cxx_fprofile_abs_path) +if(_have_cxx_fprofile_abs_path) + set(_cxx_flags "${_cxx_flags} -fprofile-abs-path") +endif() +check_c_compiler_flag("-fprofile-abs-path" _have_c_fprofile_abs_path) +if(_have_c_fprofile_abs_path) + set(_c_flags "${_c_flags} -fprofile-abs-path") +endif() + +ExternalProject_Add(microcdr_isolated + SOURCE_DIR + ${CMAKE_CURRENT_SOURCE_DIR}/../../ + BINARY_DIR + ${PROJECT_BINARY_DIR}/microcdr-build + INSTALL_DIR + ${PROJECT_BINARY_DIR}/temp_install/isolated + TEST_AFTER_INSTALL + TRUE + TEST_COMMAND + COMMAND ${CMAKE_CTEST_COMMAND} -VV -T Test + COMMAND ${CMAKE_CTEST_COMMAND} -VV -T MemCheck -E "test-case*" + COMMAND ${CMAKE_CTEST_COMMAND} -VV -T Coverage -E "test-case*" + CMAKE_CACHE_ARGS + -DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER} + -DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER} + -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} + -DCMAKE_C_FLAGS:STRING=${_c_flags} + -DCMAKE_CXX_FLAGS:STRING=${_cxx_flags} + -DCMAKE_EXE_LINKER_FLAGS:STRING=${_exe_linker_flags} + -DCMAKE_SHARED_LINKER_FLAGS:STRING=${_shared_linker_flags} + -DCMAKE_INSTALL_PREFIX:PATH= + -DUCDR_BUILD_CI_TESTS:BOOL=ON + -DGTEST_INDIVIDUAL:BOOL=ON + ) + +ExternalProject_Add(microcdr_non_isolated + SOURCE_DIR + ${CMAKE_CURRENT_SOURCE_DIR}/../../ + BINARY_DIR + ${PROJECT_BINARY_DIR}/microcdr-build + INSTALL_DIR + ${PROJECT_BINARY_DIR}/temp_install/non_isolated + TEST_AFTER_INSTALL + TRUE + BUILD_COMMAND + "" + TEST_COMMAND + COMMAND ${CMAKE_CTEST_COMMAND} -VV -T Test -R "test-case*" + CMAKE_CACHE_ARGS + -DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER} + -DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER} + -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} + -DCMAKE_C_FLAGS:STRING=${_c_flags} + -DCMAKE_CXX_FLAGS:STRING=${_cxx_flags} + -DCMAKE_EXE_LINKER_FLAGS:STRING=${_exe_linker_flags} + -DCMAKE_SHARED_LINKER_FLAGS:STRING=${_shared_linker_flags} + -DCMAKE_INSTALL_PREFIX:PATH= + -DUCDR_BUILD_CI_TESTS:BOOL=ON + -DUCDR_ISOLATED_INSTALL:BOOL=OFF + -DGTEST_INDIVIDUAL:BOOL=ON + DEPENDS + microcdr_isolated + ) \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/valgrind.supp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/valgrind.supp new file mode 100644 index 0000000..b7fb16a --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/valgrind.supp @@ -0,0 +1,17 @@ +{ + + Memcheck:Leak + match-leak-kinds: reachable + fun:*alloc + ... + obj:*ld-* + ... +} +{ + + Memcheck:Leak + fun:malloc + fun:CRYPTO_zalloc + ... + obj:*libcrypto* +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/windows/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/windows/CMakeLists.txt new file mode 100644 index 0000000..9fb300e --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/ci/windows/CMakeLists.txt @@ -0,0 +1,39 @@ +# Copyright 2019 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.5.0 FATAL_ERROR) + +project(microcdr_ci LANGUAGES C CXX) + +include(ExternalProject) + +ExternalProject_Add(microcdr + SOURCE_DIR + ${CMAKE_CURRENT_SOURCE_DIR}/../../ + BINARY_DIR + ${PROJECT_BINARY_DIR}/microcdr-build + INSTALL_DIR + ${PROJECT_BINARY_DIR}/temp_install + TEST_AFTER_INSTALL + TRUE + TEST_COMMAND + COMMAND ${CMAKE_CTEST_COMMAND} -VV -C ${CMAKE_BUILD_TYPE} -T Test + CMAKE_CACHE_ARGS + -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} + -DCMAKE_GENERATOR_TOOLSET:STRING=${CMAKE_GENERATOR_TOOLSET} + -DCMAKE_GENERATOR_PLATFORM:STRING=${CMAKE_GENERATOR_PLATFORM} + -DCMAKE_INSTALL_PREFIX:PATH= + -DUCDR_BUILD_CI_TESTS:BOOL=ON + -DGTEST_INDIVIDUAL:BOOL=ON + ) \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/SuperBuild.cmake b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/SuperBuild.cmake new file mode 100644 index 0000000..2ed22c9 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/SuperBuild.cmake @@ -0,0 +1,92 @@ +# Copyright 2019 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +include(ExternalProject) + +unset(_deps) + +enable_language(C) +enable_language(CXX) + +if(UCDR_BUILD_TESTS) + unset(googletest_DIR CACHE) + enable_language(CXX) + find_package(GTest QUIET) + find_package(GMock QUIET) + if(NOT GTest_FOUND OR NOT GMock_FOUND) + unset(GTEST_ROOT CACHE) + unset(GMOCK_ROOT CACHE) + ExternalProject_Add(googletest + GIT_REPOSITORY + https://github.com/google/googletest.git + GIT_TAG + 2fe3bd994b3189899d93f1d5a881e725e046fdc2 + PREFIX + ${PROJECT_BINARY_DIR}/googletest + INSTALL_DIR + ${PROJECT_BINARY_DIR}/temp_install/googletest + CMAKE_ARGS + -DCMAKE_INSTALL_PREFIX:PATH= + $<$:-Dgtest_force_shared_crt:BOOL=ON> + BUILD_COMMAND + COMMAND ${CMAKE_COMMAND} --build --config Release --target install + COMMAND ${CMAKE_COMMAND} --build --config Debug --target install + INSTALL_COMMAND + "" + ) + set(GTEST_ROOT ${PROJECT_BINARY_DIR}/temp_install/googletest CACHE PATH "" FORCE) + set(GMOCK_ROOT ${PROJECT_BINARY_DIR}/temp_install/googletest CACHE PATH "" FORCE) + list(APPEND _deps googletest) + endif() + + unset(fastcdr_DIR CACHE) + enable_language(CXX) + unset(FASTCDR_ROOT CACHE) + ExternalProject_Add(fastcdr + GIT_REPOSITORY + https://github.com/eProsima/Fast-CDR + GIT_TAG + v1.0.13 + PREFIX + ${PROJECT_BINARY_DIR}/fastcdr + INSTALL_DIR + ${PROJECT_BINARY_DIR}/temp_install/fastcdr + CMAKE_ARGS + -DCMAKE_INSTALL_PREFIX:PATH= + -DCMAKE_BUILD_TYPE=${DCMAKE_BUILD_TYPE} + BUILD_COMMAND + COMMAND ${CMAKE_COMMAND} --build --config Release --target install + COMMAND ${CMAKE_COMMAND} --build --config Debug --target install + INSTALL_COMMAND + "" + ) + set(FASTCDR_ROOT ${PROJECT_BINARY_DIR}/temp_install/fastcdr CACHE PATH "" FORCE) + set(FASTCDR_INCLUDE_DIRS ${PROJECT_BINARY_DIR}/temp_install/fastcdr/include CACHE PATH "" FORCE) + + list(APPEND _deps fastcdr) +endif() + +# Client project. +ExternalProject_Add(ucdr + SOURCE_DIR + ${PROJECT_SOURCE_DIR} + BINARY_DIR + ${CMAKE_CURRENT_BINARY_DIR} + CMAKE_CACHE_ARGS + -DUCDR_SUPERBUILD:BOOL=OFF + INSTALL_COMMAND + "" + DEPENDS + ${_deps} + ) diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/check_configuration.cmake b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/check_configuration.cmake new file mode 100644 index 0000000..7d98e97 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/check_configuration.cmake @@ -0,0 +1,141 @@ +# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +macro(check_stdcxx) + # Check C++11 + include(CheckCXXCompilerFlag) + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG OR + CMAKE_CXX_COMPILER_ID MATCHES "Clang") + check_cxx_compiler_flag(-std=c++14 SUPPORTS_CXX14) + set(HAVE_CXX14 0) + set(HAVE_CXX1Y 0) + set(HAVE_CXX11 0) + set(HAVE_CXX0X 0) + if(SUPPORTS_CXX14) + add_compile_options($<$:-std=c++14>) + set(HAVE_CXX14 1) + set(HAVE_CXX1Y 1) + set(HAVE_CXX11 1) + set(HAVE_CXX0X 1) + else() + check_cxx_compiler_flag(-std=c++1y SUPPORTS_CXX1Y) + if(SUPPORTS_CXX1Y) + add_compile_options($<$:-std=c++1y>) + set(HAVE_CXX1Y 1) + set(HAVE_CXX11 1) + set(HAVE_CXX0X 1) + else() + check_cxx_compiler_flag(-std=c++11 SUPPORTS_CXX11) + if(SUPPORTS_CXX11) + add_compile_options($<$:-std=c++11>) + set(HAVE_CXX11 1) + set(HAVE_CXX0X 1) + else() + check_cxx_compiler_flag(-std=c++0x SUPPORTS_CXX0X) + if(SUPPORTS_CXX0X) + add_compile_options($<$:-std=c++0x>) + set(HAVE_CXX0X 1) + else() + set(HAVE_CXX0X 0) + endif() + endif() + endif() + endif() + elseif(MSVC OR MSVC_IDE) + set(HAVE_CXX11 1) + set(HAVE_CXX0X 1) + else() + set(HAVE_CXX11 0) + set(HAVE_CXX0X 0) + endif() +endmacro() + +macro(check_compile_feature) + # Check constexpr + list(FIND CMAKE_CXX_COMPILE_FEATURES "cxx_constexpr" CXX_CONSTEXPR_SUPPORTED) + if(${CXX_CONSTEXPR_SUPPORTED} GREATER -1) + set(HAVE_CXX_CONSTEXPR 1) + else() + set(HAVE_CXX_CONSTEXPR 0) + endif() +endmacro() + +macro(check_endianness) + # Test endianness + include(TestBigEndian) + test_big_endian(BIG_ENDIAN) + set(__BIG_ENDIAN__ ${BIG_ENDIAN}) +endmacro() + +macro(check_msvc_arch) + if(MSVC_VERSION EQUAL 1700) + if(CMAKE_CL_64) + set(MSVC_ARCH "x64Win64VS2012") + else() + set(MSVC_ARCH "i86Win32VS2012") + endif() + elseif(MSVC_VERSION EQUAL 1800) + if(CMAKE_CL_64) + set(MSVC_ARCH "x64Win64VS2013") + else() + set(MSVC_ARCH "i86Win32VS2013") + endif() + elseif(MSVC_VERSION EQUAL 1900) + if(CMAKE_CL_64) + set(MSVC_ARCH "x64Win64VS2015") + else() + set(MSVC_ARCH "i86Win32VS2015") + endif() + elseif(MSVC_VERSION GREATER 1900) + if(CMAKE_CL_64) + set(MSVC_ARCH "x64Win64VS2017") + else() + set(MSVC_ARCH "i86Win32VS2017") + endif() + else() + if(CMAKE_CL_64) + set(MSVC_ARCH "x64Win64VSUnknown") + else() + set(MSVC_ARCH "i86Win32VSUnknown") + endif() + endif() +endmacro() + +function(set_common_compile_options target) + enable_language(C) + enable_language(CXX) + if(MSVC OR MSVC_IDE) + target_compile_options(${target} PRIVATE /W4) + else() + target_compile_options(${target} PRIVATE -Wall + -Wextra + -Wshadow + $<$:-Wnon-virtual-dtor> + -pedantic + -Wcast-align + -Wunused + $<$:-Woverloaded-virtual> + -Wconversion + -Wsign-conversion + $<$:-Wlogical-op> + $<$,$>:-Wuseless-cast> + -Wdouble-promotion + $<$:-Wold-style-cast> + $<$,$,6.0.0>>>,$,$,6.0.0>>>>:-Wnull-dereference> + $<$,$,7.0.0>>>,$,$,7.0.0>>>>:-Wduplicated-branches> + $<$,$,6.0.0>>>,$,$,6.0.0>>>>:-Wduplicated-cond> + $<$,$,7.0.0>>>,$,$,7.0.0>>>>:-Wrestrict> + ) + endif() +endfunction() diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/eprosima_libraries.cmake b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/eprosima_libraries.cmake new file mode 100644 index 0000000..108b70d --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/eprosima_libraries.cmake @@ -0,0 +1,104 @@ +# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +macro(eprosima_find_package package) + + if(NOT ${package}_FOUND) + + # Parse arguments. + set(options REQUIRED) + set(multiValueArgs OPTIONS) + cmake_parse_arguments(FIND "${options}" "" "${multiValueArgs}" ${ARGN}) + + option(THIRDPARTY "Activate the use of internal thirdparties" OFF) + option(THIRDPARTY_UPDATE "Activate the auto update of internal thirdparties" ON) + + if(EPROSIMA_BUILD) + set(THIRDPARTY ON) + endif() + + option(THIRDPARTY_${package} "Activate the use of internal thirdparty ${package}" OFF) + + if(NOT EPROSIMA_INSTALLER) + find_package(${package} QUIET) + endif() + + if(NOT ${package}_FOUND AND (THIRDPARTY OR THIRDPARTY_${package})) + set(SUBDIRECTORY_EXIST TRUE) + if(THIRDPARTY_UPDATE OR NOT EXISTS "${PROJECT_SOURCE_DIR}/thirdparty/${package}/CMakeLists.txt") + message(STATUS "${package} thirdparty is being updated...") + execute_process( + COMMAND git submodule update --recursive --init "thirdparty/${package}" + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + RESULT_VARIABLE EXECUTE_RESULT + ) + if(NOT EXECUTE_RESULT EQUAL 0) + message(WARNING "Cannot configure Git submodule ${package}") + if(NOT EXISTS "${PROJECT_SOURCE_DIR}/thirdparty/${package}/CMakeLists.txt") + set(SUBDIRECTORY_EXIST FALSE) + endif() + endif() + endif() + + if(SUBDIRECTORY_EXIST) + foreach(opt_ ${FIND_OPTIONS}) + set(${opt_} ON) + endforeach() + # Keep temp value in order to avoid call packages' installer. + set(EPROSIMA_INSTALLER_TEMP ${EPROSIMA_INSTALLER}) + unset(EPROSIMA_INSTALLER CACHE) + add_subdirectory(${PROJECT_SOURCE_DIR}/thirdparty/${package}) + set(EPROSIMA_INSTALLER ${EPROSIMA_INSTALLER_TEMP}) + set(${package}_FOUND TRUE) + if(NOT IS_TOP_LEVEL) + set(${package}_FOUND TRUE PARENT_SCOPE) + endif() + endif() + endif() + + if(${package}_FOUND) + message(STATUS "${package} library found...") + elseif(${FIND_REQUIRED}) + message(FATAL_ERROR "${package} library not found...") + else() + message(STATUS "${package} library not found...") + endif() + endif() +endmacro() + +macro(eprosima_find_thirdparty package thirdparty_name) + if(NOT (EPROSIMA_INSTALLER AND (MSVC OR MSVC_IDE))) + + option(THIRDPARTY_${package} "Activate the use of internal thirdparty ${package}" OFF) + + find_package(${package} QUIET) + + if(NOT ${package}_FOUND AND (THIRDPARTY OR THIRDPARTY_${package})) + execute_process( + COMMAND git submodule update --recursive --init "thirdparty/${thirdparty_name}" + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + RESULT_VARIABLE EXECUTE_RESULT + ) + + if(EXECUTE_RESULT EQUAL 0) + set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} ${PROJECT_SOURCE_DIR}/thirdparty/${thirdparty_name}) + set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} ${PROJECT_SOURCE_DIR}/thirdparty/${thirdparty_name}/${thirdparty_name}) + find_package(${package} REQUIRED) + else() + message(FATAL_ERROR "Cannot configure Git submodule ${package}") + endif() + endif() + + endif() +endmacro() diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/gtest.cmake b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/gtest.cmake new file mode 100644 index 0000000..b8b8fa1 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/common/gtest.cmake @@ -0,0 +1,173 @@ +# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +option(GTEST_INDIVIDUAL "Activate the execution of GTest tests" OFF) + +macro(check_gtest) + if(NOT GTEST_FOUND) + if(WIN32) + option(EPROSIMA_GTEST "Activate special set of GTEST_ROOT" OFF) + if(EPROSIMA_BUILD) + set(EPROSIMA_GTEST ON) + endif() + endif() + + # Find package GTest + if(WIN32 AND EPROSIMA_GTEST) + if(NOT GTEST_ROOT) + set(GTEST_ROOT_ $ENV{GTEST_ROOT}) + if(GTEST_ROOT_) + file(TO_CMAKE_PATH "${GTEST_ROOT_}/${MSVC_ARCH}" GTEST_ROOT) + endif() + else() + file(TO_CMAKE_PATH "${GTEST_ROOT}/${MSVC_ARCH}" GTEST_ROOT) + endif() + endif() + find_package(GTest) + + if(GTEST_FOUND) + find_package(Threads REQUIRED) + set(GTEST_LIBRARIES ${GTEST_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) + set(GTEST_BOTH_LIBRARIES ${GTEST_BOTH_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) + endif() + endif() +endmacro() + +macro(check_gmock) + if(NOT GMOCK_FOUND) + if(WIN32) + option(EPROSIMA_GMOCK "Activate special set of GMOCK_ROOT" OFF) + if(EPROSIMA_BUILD) + set(EPROSIMA_GMOCK ON) + endif() + endif() + + # Find package GMock + if(WIN32 AND EPROSIMA_GMOCK) + if(NOT GMOCK_ROOT) + set(GMOCK_ROOT_ $ENV{GMOCK_ROOT}) + if(GMOCK_ROOT_) + file(TO_CMAKE_PATH "${GMOCK_ROOT_}/${MSVC_ARCH}" GMOCK_ROOT) + endif() + else() + file(TO_CMAKE_PATH "${GMOCK_ROOT}/${MSVC_ARCH}" GMOCK_ROOT) + endif() + endif() + find_package(GMock) + + if(GMOCK_FOUND) + find_package(Threads REQUIRED) + set(GMOCK_LIBRARIES ${GMOCK_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) + set(GMOCK_BOTH_LIBRARIES ${GMOCK_BOTH_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) + endif() + endif() +endmacro() + +macro(add_gtest test) + # Parse arguments + set(multiValueArgs SOURCES ENVIRONMENTS DEPENDENCIES) + cmake_parse_arguments(GTEST "" "" "${multiValueArgs}" ${ARGN}) + + if(GTEST_INDIVIDUAL) + if(WIN32) + set(WIN_PATH "$ENV{PATH}") + get_target_property(LINK_LIBRARIES_ ${test} LINK_LIBRARIES) + if(NOT "${LINK_LIBRARIES_}" STREQUAL "LINK_LIBRARIES_-NOTFOUND") + foreach(LIBRARY_LINKED ${LINK_LIBRARIES_}) + if(TARGET ${LIBRARY_LINKED}) + set(WIN_PATH "$;${WIN_PATH}") + endif() + endforeach() + endif() + foreach(DEP ${GTEST_DEPENDENCIES}) + set(WIN_PATH "$;${WIN_PATH}") + endforeach() + string(REPLACE ";" "\\;" WIN_PATH "${WIN_PATH}") + endif() + + foreach(GTEST_SOURCE_FILE ${GTEST_SOURCES}) + file(STRINGS ${GTEST_SOURCE_FILE} GTEST_NAMES REGEX ^TEST) + + # Search for google value-parameterized tests instantiations + unset(GTEST_INSTANTATIONS) # list of instantiations names + file(STRINGS ${GTEST_SOURCE_FILE} GTEST_INSTANTIATION_NAMES REGEX ^INSTANTIATE_TEST_CASE_P) + foreach(GTEST_INSTANTIATION_NAME ${GTEST_INSTANTIATION_NAMES}) + # Search and append all instantiation names + string(REGEX REPLACE ["\) \(,"] ";" GTEST_INSTANTIATION_NAME ${GTEST_INSTANTIATION_NAME}) + list(GET GTEST_INSTANTIATION_NAME 1 GTEST_INSTANTIATION_NAME) + list(APPEND GTEST_INSTANTATIONS ${GTEST_INSTANTIATION_NAME}) + endforeach() + + foreach(GTEST_NAME ${GTEST_NAMES}) + unset(GTEST_TEMPLATE_TEST) # flag if current test is a value-parametrized one + string(REGEX MATCH ^TEST_P GTEST_TEMPLATE_TEST ${GTEST_NAME}) + string(REGEX REPLACE ["\) \(,"] ";" GTEST_NAME ${GTEST_NAME}) + + list(GET GTEST_NAME 1 GTEST_GROUP_NAME) + list(GET GTEST_NAME 3 GTEST_NAME) + if (GTEST_INSTANTATIONS AND GTEST_TEMPLATE_TEST) + # In case is a value-paremetrized test and there are instantiations generate test calls per instantiation + # Note the /*. This test will execute all the parameters combinations + foreach(GTEST_INSTATATION_NAME ${GTEST_INSTANTATIONS}) + add_test(NAME ${GTEST_INSTATATION_NAME}.${GTEST_GROUP_NAME}.${GTEST_NAME} + COMMAND ${test} + --gtest_filter=${GTEST_INSTATATION_NAME}/${GTEST_GROUP_NAME}.${GTEST_NAME}/*) + # Add environment + if(WIN32) + set_property(TEST ${GTEST_INSTATATION_NAME}.${GTEST_GROUP_NAME}.${GTEST_NAME} APPEND PROPERTY ENVIRONMENT "PATH=${WIN_PATH}") + endif() + endforeach() + + else() + add_test(NAME ${GTEST_GROUP_NAME}.${GTEST_NAME} + COMMAND ${test} + --gtest_filter=${GTEST_GROUP_NAME}.${GTEST_NAME}) + # Add environment + if(WIN32) + set_property(TEST ${GTEST_GROUP_NAME}.${GTEST_NAME} APPEND PROPERTY ENVIRONMENT "PATH=${WIN_PATH}") + endif() + endif() + + + foreach(property ${GTEST_ENVIRONMENTS}) + set_property(TEST ${GTEST_GROUP_NAME}.${GTEST_NAME} APPEND PROPERTY ENVIRONMENT "${property}") + endforeach() + endforeach() + endforeach() + else() + add_test(NAME ${test} COMMAND ${test}) + + # Add environment + if(WIN32) + set(WIN_PATH "$ENV{PATH}") + get_target_property(LINK_LIBRARIES_ ${test} LINK_LIBRARIES) + if(NOT "${LINK_LIBRARIES_}" STREQUAL "LINK_LIBRARIES_-NOTFOUND") + foreach(LIBRARY_LINKED ${LINK_LIBRARIES_}) + if(TARGET ${LIBRARY_LINKED}) + set(WIN_PATH "$;${WIN_PATH}") + endif() + endforeach() + endif() + foreach(DEP ${GTEST_DEPENDENCIES}) + set(WIN_PATH "$;${WIN_PATH}") + endforeach() + string(REPLACE ";" "\\;" WIN_PATH "${WIN_PATH}") + set_property(TEST ${test} APPEND PROPERTY ENVIRONMENT "PATH=${WIN_PATH}") + endif() + + foreach(property ${GTEST_ENVIRONMENTS}) + set_property(TEST ${test} APPEND PROPERTY ENVIRONMENT "${property}") + endforeach() + endif() +endmacro() diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/packaging/Config.cmake.in b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/packaging/Config.cmake.in new file mode 100644 index 0000000..3e00a3d --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/cmake/packaging/Config.cmake.in @@ -0,0 +1,26 @@ +# Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set(@PROJECT_NAME@_VERSION @PROJECT_VERSION@) + +@PACKAGE_INIT@ + +if((MSVC OR MSVC_IDE) AND EXISTS "@PACKAGE_BIN_INSTALL_DIR@") + set_and_check(@PROJECT_NAME@_BIN_DIR "@PACKAGE_BIN_INSTALL_DIR@") +endif() +set_and_check(@PROJECT_NAME@_INCLUDE_DIR "@PACKAGE_INCLUDE_INSTALL_DIR@") +set_and_check(@PROJECT_NAME@_LIB_DIR "@PACKAGE_LIB_INSTALL_DIR@") +set_and_check(@PROJECT_NAME@_DATA_DIR "@PACKAGE_DATA_INSTALL_DIR@") + +include(${@PROJECT_NAME@_DATA_DIR}/@PROJECT_NAME@/cmake/@PROJECT_NAME@Targets.cmake) diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/colcon.pkg b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/colcon.pkg new file mode 100644 index 0000000..f17eda9 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/colcon.pkg @@ -0,0 +1,7 @@ +{ + "name": "microcdr", + "type": "cmake", + "cmake-args":[ + "-DUCDR_ISOLATED_INSTALL=OFF" + ] +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/basic/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/basic/CMakeLists.txt new file mode 100644 index 0000000..6605cce --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/basic/CMakeLists.txt @@ -0,0 +1,23 @@ +project(basic_example) + +add_executable(${PROJECT_NAME} basic.c) +if(MSVC OR MSVC_IDE) + target_compile_options(${PROJECT_NAME} PRIVATE /wd4996) +endif() + +set_target_properties(${PROJECT_NAME} PROPERTIES + C_STANDARD 99 + C_STANDARD_REQUIRED YES + ) + +target_link_libraries(${PROJECT_NAME} microcdr) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION examples/ucdr/${PROJECT_NAME}/${BIN_INSTALL_DIR} + ) + +install(DIRECTORY ${PROJECT_SOURCE_DIR}/ + DESTINATION examples/ucdr/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" + PATTERN "*.c" + ) diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/basic/basic.c b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/basic/basic.c new file mode 100644 index 0000000..d9f2280 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/basic/basic.c @@ -0,0 +1,45 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#define BUFFER_LENGTH 256 + +int main() +{ + // Data buffer + uint8_t buffer[BUFFER_LENGTH]; + + // Structs for handle the buffer. + ucdrBuffer writer; + ucdrBuffer reader; + + // Initialize the MicroBuffers for working with an user-managed buffer. + ucdr_init_buffer(&writer, buffer, BUFFER_LENGTH); + ucdr_init_buffer(&reader, buffer, BUFFER_LENGTH); + + // Serialize data + char input[16] = "Hello MicroCDR!"; //16 characters + ucdr_serialize_array_char(&writer, input, 16); + + // Deserialize data + char output[16]; + ucdr_deserialize_array_char(&reader, output, 16); + + printf("Input: %s\n", input); + printf("Output: %s\n", output); + + return 0; +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/fragmentation/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/fragmentation/CMakeLists.txt new file mode 100644 index 0000000..7ff177e --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/fragmentation/CMakeLists.txt @@ -0,0 +1,23 @@ +project(fragmentation_example) + +add_executable(${PROJECT_NAME} fragmentation.c) +if(MSVC OR MSVC_IDE) + target_compile_options(${PROJECT_NAME} PRIVATE /wd4996) +endif() + +set_target_properties(${PROJECT_NAME} PROPERTIES + C_STANDARD 99 + C_STANDARD_REQUIRED YES + ) + +target_link_libraries(${PROJECT_NAME} microcdr) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION examples/ucdr/${PROJECT_NAME}/${BIN_INSTALL_DIR} + ) + +install(DIRECTORY ${PROJECT_SOURCE_DIR}/ + DESTINATION examples/ucdr/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" + PATTERN "*.c" + ) diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/fragmentation/fragmentation.c b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/fragmentation/fragmentation.c new file mode 100644 index 0000000..78f9bf7 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/examples/fragmentation/fragmentation.c @@ -0,0 +1,85 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#define BUFFER_LENGTH 12 +#define SLOTS 12 +#define STRING_MAX 128 + +bool on_full_buffer( + ucdrBuffer* ub, + void* args) +{ + uint8_t* buffer = (uint8_t*) args; + + // This value correspong with the ub->error, and will be returned by this function to indicates + // if the serialization must continue or must stop because of an error. + bool error = true; + + // Leave the odd slots empty. + uint32_t next_slot = 2 + (uint32_t)(ub->init - buffer) / BUFFER_LENGTH; + if (next_slot < SLOTS) + { + // Modify the internal buffer + ub->init = buffer + BUFFER_LENGTH * next_slot; + ub->iterator = ub->init; + ub->final = ub->init + BUFFER_LENGTH; + + printf(" Extend buffer to slot %u\n", next_slot); + + // As we want to continue the serialization without errors, we return false. + error = false; + } + + return error; +} + +int main() +{ + // Data buffer + uint8_t buffer[SLOTS * BUFFER_LENGTH]; + + // Structs for handle the buffer. + ucdrBuffer writer; + ucdrBuffer reader; + + // Initialize the MicroBuffers for working with an user-managed buffer. + ucdr_init_buffer(&writer, buffer, BUFFER_LENGTH); + ucdr_init_buffer(&reader, buffer, BUFFER_LENGTH); + + // Add a full buffer behavior to the writer and the reader + ucdr_set_on_full_buffer_callback(&writer, on_full_buffer, buffer); + ucdr_set_on_full_buffer_callback(&reader, on_full_buffer, buffer); + + // Serialize data + printf("Serializing...\n"); + char input[STRING_MAX] = "Hello MicroCDR! this message is fragmented"; + ucdr_serialize_string(&writer, input); + printf("\n"); + + // Deserialize data + printf("Deserializing...\n"); + char output[STRING_MAX]; + ucdr_deserialize_string(&reader, output, STRING_MAX); + printf("\n"); + + printf("Input: %s\n", input); + printf("Output: %s\n", output); + + return 0; +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/config.h.in b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/config.h.in new file mode 100644 index 0000000..98b12b8 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/config.h.in @@ -0,0 +1,27 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _MICROCDR_CONFIG_H_ +#define _MICROCDR_CONFIG_H_ + +// Version defines +#define MICROCDR_VERSION_MAJOR @PROJECT_VERSION_MAJOR@ +#define MICROCDR_VERSION_MINOR @PROJECT_VERSION_MINOR@ +#define MICROCDR_VERSION_MICRO @PROJECT_VERSION_PATCH@ +#define MICROCDR_VERSION_STR "@PROJECT_VERSION@" + +// ucdrEndianness defines +#define UCDR_MACHINE_ENDIANNESS @CONFIG_MACHINE_ENDIANNESS@ + +#endif // _MICROCDR_CONFIG_H_ diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/microcdr.h b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/microcdr.h new file mode 100644 index 0000000..a665fd5 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/microcdr.h @@ -0,0 +1,219 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _MICROCDR_MICRO_CDR_H_ +#define _MICROCDR_MICRO_CDR_H_ + +#ifdef __cplusplus +extern "C" { +#endif // ifdef __cplusplus + +#include +#include + +#include +#include +#include + +// ------------------------------------------------ +// Types +// ------------------------------------------------ + +struct ucdrBuffer; +typedef bool (* OnFullBuffer)( + struct ucdrBuffer* buffer, + void* args); + +typedef enum ucdrEndianness +{ + UCDR_BIG_ENDIANNESS = 0, + UCDR_LITTLE_ENDIANNESS = 1 + +} ucdrEndianness; + +typedef struct ucdrBuffer +{ + uint8_t* init; + uint8_t* final; + uint8_t* iterator; + + size_t origin; + size_t offset; + + ucdrEndianness endianness; + uint8_t last_data_size; + + bool error; + + OnFullBuffer on_full_buffer; + void* args; + +} ucdrBuffer; + +// ------------------------------------------------ +// Main functions +// ------------------------------------------------ + +UCDRDLLAPI void ucdr_init_buffer ( + ucdrBuffer* ub, + uint8_t* data, + size_t size); +UCDRDLLAPI void ucdr_init_buffer_origin ( + ucdrBuffer* ub, + uint8_t* data, + size_t size, + size_t origin); +UCDRDLLAPI void ucdr_init_buffer_origin_offset ( + ucdrBuffer* ub, + uint8_t* data, + size_t size, + size_t origin, + size_t offset); +UCDRDLLAPI void ucdr_init_buffer_origin_offset_endian ( + ucdrBuffer* ub, + uint8_t* data, + size_t size, + size_t origin, + size_t offset, + ucdrEndianness endianness); +UCDRDLLAPI void ucdr_copy_buffer ( + ucdrBuffer* ub_dest, + const ucdrBuffer* ub_source); +UCDRDLLAPI void ucdr_set_on_full_buffer_callback ( + ucdrBuffer* ub, + OnFullBuffer on_full_buffer, + void* args); + +UCDRDLLAPI void ucdr_reset_buffer ( + ucdrBuffer* ub); +UCDRDLLAPI void ucdr_reset_buffer_offset ( + ucdrBuffer* ub, + size_t offset); + +UCDRDLLAPI void ucdr_align_to ( + ucdrBuffer* ub, + size_t alignment); +UCDRDLLAPI size_t ucdr_alignment ( + size_t buffer_position, + size_t data_size); +UCDRDLLAPI size_t ucdr_buffer_alignment ( + const ucdrBuffer* ub, + size_t data_size); +UCDRDLLAPI void ucdr_advance_buffer ( + ucdrBuffer* ub, + size_t size); + +UCDRDLLAPI size_t ucdr_buffer_size ( + const ucdrBuffer* ub); +UCDRDLLAPI size_t ucdr_buffer_length ( + const ucdrBuffer* ub); +UCDRDLLAPI size_t ucdr_buffer_remaining ( + const ucdrBuffer* ub); +UCDRDLLAPI ucdrEndianness ucdr_buffer_endianness ( + const ucdrBuffer* ub); +UCDRDLLAPI bool ucdr_buffer_has_error ( + const ucdrBuffer* ub); + +// ------------------------------------------------------------------- +// PUBLIC DE-SERIALIZATION DECLARATIONS +// ------------------------------------------------------------------- + +#define UCDR_BASIC_TYPE_DECLARATIONS(SUFFIX, TYPE) \ + UCDRDLLAPI bool ucdr_serialize ## SUFFIX(ucdrBuffer * ub, TYPE value); \ + UCDRDLLAPI bool ucdr_serialize_endian ## SUFFIX(ucdrBuffer * ub, ucdrEndianness endianness, TYPE value); \ + UCDRDLLAPI bool ucdr_deserialize ## SUFFIX(ucdrBuffer * ub, TYPE * value); \ + UCDRDLLAPI bool ucdr_deserialize_endian ## SUFFIX(ucdrBuffer * ub, ucdrEndianness endianness, TYPE * value); \ + +#define UCDR_ARRAY_DECLARATIONS(SUFFIX, TYPE) \ + UCDRDLLAPI bool ucdr_serialize_array ## SUFFIX(ucdrBuffer * ub, const TYPE * array, size_t size); \ + UCDRDLLAPI bool ucdr_serialize_endian_array ## SUFFIX(ucdrBuffer * ub, ucdrEndianness endianness, \ + const TYPE * array, size_t size); \ + UCDRDLLAPI bool ucdr_deserialize_array ## SUFFIX(ucdrBuffer * ub, TYPE * array, size_t size); \ + UCDRDLLAPI bool ucdr_deserialize_endian_array ## SUFFIX(ucdrBuffer * ub, ucdrEndianness endianness, TYPE * array, \ + size_t size); \ + +#define UCDR_SEQUENCE_DECLARATIONS(SUFFIX, TYPE) \ + UCDRDLLAPI bool ucdr_serialize_sequence ## SUFFIX(ucdrBuffer * ub, const TYPE * array, uint32_t length); \ + UCDRDLLAPI bool ucdr_serialize_endian_sequence ## SUFFIX(ucdrBuffer * ub, ucdrEndianness endianness, \ + const TYPE * array, uint32_t length); \ + UCDRDLLAPI bool ucdr_deserialize_sequence ## SUFFIX(ucdrBuffer * ub, TYPE * array, size_t array_capacity, \ + uint32_t * length); \ + UCDRDLLAPI bool ucdr_deserialize_endian_sequence ## SUFFIX(ucdrBuffer * ub, ucdrEndianness endianness, TYPE * array, \ + size_t array_capacity, uint32_t * length); \ + +UCDRDLLAPI bool ucdr_serialize_string( + ucdrBuffer* ub, + const char* string); +UCDRDLLAPI bool ucdr_serialize_endian_string( + ucdrBuffer* ub, + ucdrEndianness endianness, + const char* string); +UCDRDLLAPI bool ucdr_deserialize_string( + ucdrBuffer* ub, + char* string, + size_t string_capacity); +UCDRDLLAPI bool ucdr_deserialize_endian_string( + ucdrBuffer* ub, + ucdrEndianness endianness, + char* string, + size_t string_capacity); + +// ------------------------------------------------------------------- +// VALID TYPES DECLARATIONS +// ------------------------------------------------------------------- + +UCDR_BASIC_TYPE_DECLARATIONS(_char, char) +UCDR_BASIC_TYPE_DECLARATIONS(_bool, bool) +UCDR_BASIC_TYPE_DECLARATIONS(_uint8_t, uint8_t) +UCDR_BASIC_TYPE_DECLARATIONS(_uint16_t, uint16_t) +UCDR_BASIC_TYPE_DECLARATIONS(_uint32_t, uint32_t) +UCDR_BASIC_TYPE_DECLARATIONS(_uint64_t, uint64_t) +UCDR_BASIC_TYPE_DECLARATIONS(_int8_t, int8_t) +UCDR_BASIC_TYPE_DECLARATIONS(_int16_t, int16_t) +UCDR_BASIC_TYPE_DECLARATIONS(_int32_t, int32_t) +UCDR_BASIC_TYPE_DECLARATIONS(_int64_t, int64_t) +UCDR_BASIC_TYPE_DECLARATIONS(_float, float) +UCDR_BASIC_TYPE_DECLARATIONS(_double, double) + +UCDR_ARRAY_DECLARATIONS(_char, char) +UCDR_ARRAY_DECLARATIONS(_bool, bool) +UCDR_ARRAY_DECLARATIONS(_uint8_t, uint8_t) +UCDR_ARRAY_DECLARATIONS(_uint16_t, uint16_t) +UCDR_ARRAY_DECLARATIONS(_uint32_t, uint32_t) +UCDR_ARRAY_DECLARATIONS(_uint64_t, uint64_t) +UCDR_ARRAY_DECLARATIONS(_int8_t, int8_t) +UCDR_ARRAY_DECLARATIONS(_int16_t, int16_t) +UCDR_ARRAY_DECLARATIONS(_int32_t, int32_t) +UCDR_ARRAY_DECLARATIONS(_int64_t, int64_t) +UCDR_ARRAY_DECLARATIONS(_float, float) +UCDR_ARRAY_DECLARATIONS(_double, double) + +UCDR_SEQUENCE_DECLARATIONS(_char, char) +UCDR_SEQUENCE_DECLARATIONS(_bool, bool) +UCDR_SEQUENCE_DECLARATIONS(_uint8_t, uint8_t) +UCDR_SEQUENCE_DECLARATIONS(_uint16_t, uint16_t) +UCDR_SEQUENCE_DECLARATIONS(_uint32_t, uint32_t) +UCDR_SEQUENCE_DECLARATIONS(_uint64_t, uint64_t) +UCDR_SEQUENCE_DECLARATIONS(_int8_t, int8_t) +UCDR_SEQUENCE_DECLARATIONS(_int16_t, int16_t) +UCDR_SEQUENCE_DECLARATIONS(_int32_t, int32_t) +UCDR_SEQUENCE_DECLARATIONS(_int64_t, int64_t) +UCDR_SEQUENCE_DECLARATIONS(_float, float) +UCDR_SEQUENCE_DECLARATIONS(_double, double) + +#ifdef __cplusplus +} +#endif // ifdef __cplusplus + +#endif //_MICROCDR_MICRO_CDR_H_ diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/visibility.h b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/visibility.h new file mode 100644 index 0000000..fb5fa6e --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/include/ucdr/visibility.h @@ -0,0 +1,33 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef _MICROCDR_VISIBILITY_H_ +#define _MICROCDR_VISIBILITY_H_ + +#if defined(_WIN32) +#if defined(microcdr_SHARED) +#if defined(microcdr_EXPORTS) +#define UCDRDLLAPI __declspec( dllexport ) +#else +#define UCDRDLLAPI __declspec( dllimport ) +#endif // microcdr_EXPORTS +#else +#define UCDRDLLAPI +#endif // BUILDING_SHARED_LIBS +#else +#define UCDRDLLAPI +#endif // _WIN32 + +#endif // _MICROCDR_VISIBILITY_H_ diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/src/c/common.c b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/src/c/common.c new file mode 100644 index 0000000..cae5a60 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/src/c/common.c @@ -0,0 +1,235 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "common_internal.h" + +#include + +static size_t ucdr_next_remaining_size( + ucdrBuffer* ub, + size_t bytes, + size_t data_size); + +// ------------------------------------------------------------------- +// INTERNAL UTIL IMPLEMENTATIONS +// ------------------------------------------------------------------- + +bool ucdr_check_buffer_available_for( + ucdrBuffer* ub, + size_t bytes) +{ + return !ub->error && (ub->iterator + bytes <= ub->final); +} + +bool ucdr_check_final_buffer_behavior( + ucdrBuffer* ub, + size_t data_size) +{ + if (!ub->error && ub->iterator + data_size > ub->final) + { + ub->error = (NULL != ub->on_full_buffer) ? ub->on_full_buffer(ub, ub->args) : true; + } + + return !ub->error; +} + +size_t ucdr_next_remaining_size( + ucdrBuffer* ub, + size_t bytes, + size_t data_size) +{ + (void) data_size; + size_t remaining = ucdr_buffer_remaining(ub); + return (bytes <= remaining) ? bytes : remaining; +} + +size_t ucdr_check_final_buffer_behavior_array( + ucdrBuffer* ub, + size_t bytes, + size_t data_size) +{ + if (!ub->error && ub->iterator + data_size > ub->final && bytes > 0) + { + ub->error = (NULL != ub->on_full_buffer) ? ub->on_full_buffer(ub, ub->args) : true; + } + + return (!ub->error) ? ucdr_next_remaining_size(ub, bytes, data_size) : 0; +} + +void ucdr_set_on_full_buffer_callback( + ucdrBuffer* ub, + OnFullBuffer on_full_buffer, + void* args) +{ + ub->on_full_buffer = on_full_buffer; + ub->args = args; +} + +// ------------------------------------------------------------------- +// PUBLIC IMPLEMENTATION +// ------------------------------------------------------------------- +void ucdr_init_buffer( + ucdrBuffer* ub, + uint8_t* data, + size_t size) +{ + ucdr_init_buffer_origin(ub, data, size, 0u); +} + +void ucdr_init_buffer_origin( + ucdrBuffer* ub, + uint8_t* data, + size_t size, + size_t origin) +{ + ucdr_init_buffer_origin_offset(ub, data, size, origin, 0u); +} + +void ucdr_init_buffer_origin_offset( + ucdrBuffer* ub, + uint8_t* data, + size_t size, + size_t origin, + size_t offset) +{ + ucdr_init_buffer_origin_offset_endian(ub, data, size, origin, offset, UCDR_MACHINE_ENDIANNESS); +} + +void ucdr_init_buffer_origin_offset_endian( + ucdrBuffer* ub, + uint8_t* data, + size_t size, + size_t origin, + size_t offset, + ucdrEndianness endianness) +{ + ub->init = data; + ub->final = ub->init + size; + ub->iterator = ub->init + offset; + ub->origin = origin; + ub->offset = origin + offset; + ub->endianness = endianness; + ub->last_data_size = 0u; + ub->error = false; + ub->on_full_buffer = NULL; + ub->args = NULL; +} + +void ucdr_copy_buffer( + ucdrBuffer* ub_dest, + const ucdrBuffer* ub_source) +{ + memcpy(ub_dest, ub_source, sizeof(ucdrBuffer)); +} + +void ucdr_reset_buffer( + ucdrBuffer* ub) +{ + ucdr_reset_buffer_offset(ub, 0u); +} + +void ucdr_reset_buffer_offset( + ucdrBuffer* ub, + size_t offset) +{ + ub->iterator = ub->init + offset; + ub->offset = ub->origin + offset; + ub->last_data_size = 0u; + ub->error = false; +} + +void ucdr_align_to( + ucdrBuffer* ub, + size_t size) +{ + size_t alignment = ucdr_buffer_alignment(ub, size); + ub->offset += alignment; + + // TODO (julibert): rethink. + ub->iterator += alignment; + if (ub->iterator > ub->final) + { + ub->iterator = ub->final; + } + ub->last_data_size = (uint8_t)size; +} + +size_t ucdr_alignment( + size_t current_alignment, + size_t data_size) +{ + return ((data_size - (current_alignment % data_size)) & (data_size - 1)); +} + +size_t ucdr_buffer_alignment( + const ucdrBuffer* ub, + size_t data_size) +{ + return (data_size > ub->last_data_size) + ? (data_size - ((uint32_t)(ub->offset - ub->origin) % data_size)) & (data_size - 1) + : 0; +} + +void ucdr_advance_buffer( + ucdrBuffer* ub, + size_t size) +{ + if (ucdr_check_buffer_available_for(ub, size)) + { + ub->iterator += size; + ub->offset += size; + } + else + { + size_t remaining_size = size; + size_t serialization_size; + while (0 < (serialization_size = ucdr_check_final_buffer_behavior_array(ub, remaining_size, 1))) + { + remaining_size -= serialization_size; + ub->iterator += serialization_size; + ub->offset += serialization_size; + } + } + ub->last_data_size = 1; +} + +size_t ucdr_buffer_size( + const ucdrBuffer* ub) +{ + return (size_t)(ub->final - ub->init); +} + +size_t ucdr_buffer_length( + const ucdrBuffer* ub) +{ + return (size_t)(ub->iterator - ub->init); +} + +size_t ucdr_buffer_remaining( + const ucdrBuffer* ub) +{ + return (size_t)(ub->final - ub->iterator); +} + +ucdrEndianness ucdr_buffer_endianness( + const ucdrBuffer* ub) +{ + return ub->endianness; +} + +bool ucdr_buffer_has_error( + const ucdrBuffer* ub) +{ + return ub->error; +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/src/c/common_internal.h b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/src/c/common_internal.h new file mode 100644 index 0000000..565b734 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/src/c/common_internal.h @@ -0,0 +1,42 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _SRC_COMMON_INTERNAL_H_ +#define _SRC_COMMON_INTERNAL_H_ + +#ifdef __cplusplus +extern "C" { +#endif // ifdef __cplusplus + +#include + +// ------------------------------------------------------------------- +// INTERNAL UTIL FUNCTIONS +// ------------------------------------------------------------------- +bool ucdr_check_buffer_available_for( + ucdrBuffer* ub, + size_t bytes); +bool ucdr_check_final_buffer_behavior( + ucdrBuffer* ub, + size_t bytes); +size_t ucdr_check_final_buffer_behavior_array( + ucdrBuffer* ub, + size_t bytes, + size_t data_size); + +#ifdef __cplusplus +} +#endif // ifdef __cplusplus + +#endif //_SRC_COMMON_INTERNAL_H_ diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/Alignment.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/Alignment.cpp new file mode 100644 index 0000000..736d817 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/Alignment.cpp @@ -0,0 +1,67 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "serialization/BasicSerialization.hpp" + +class Alignment : public BasicSerialization, public ::testing::WithParamInterface +{ +public: + + Alignment() + { + int offset = GetParam(); + for (int i = 0; i < offset; ++i) + { + uint8_t_serialization(); + } + } + + void check_alignment( + int alignment) + { + EXPECT_EQ(static_cast(ucdr_buffer_length(&writer)) % alignment, 0); + EXPECT_EQ(static_cast(ucdr_buffer_length(&reader)) % alignment, 0); + } + + virtual ~Alignment() + { + } + +}; + +INSTANTIATE_TEST_CASE_P(Offset, Alignment, ::testing::Range(0, 17), ::testing::PrintToStringParamName()); + +TEST_P(Alignment, Block_8) +{ + uint64_t_serialization(); + check_alignment(8); +} + +TEST_P(Alignment, Block_4) +{ + uint32_t_serialization(); + check_alignment(4); +} + +TEST_P(Alignment, Block_2) +{ + uint16_t_serialization(); + check_alignment(2); +} + +TEST_P(Alignment, Block_1) +{ + uint8_t_serialization(); + check_alignment(1); +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/CMakeLists.txt new file mode 100644 index 0000000..2fedd00 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/CMakeLists.txt @@ -0,0 +1,63 @@ +# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 2.8.12 FATAL_ERROR) + +# New project because of the CXX GTest dependency. +if(CMAKE_VERSION VERSION_LESS 3.0) + project(microcdr_tests C CXX) +else() + cmake_policy(SET CMP0048 NEW) + project(microcdr_tests LANGUAGES C CXX) +endif() + +check_gtest() +if(GTEST_FOUND) + include(CTest) + find_package(fastcdr REQUIRED) + set(SRCS + CommonFunctions.cpp + serialization/BasicSerialization.cpp + serialization/ArraySerialization.cpp + serialization/SequenceSerialization.cpp + serialization/StringSerialization.cpp + endianness/BasicEndianness.cpp + endianness/ArrayEndianness.cpp + endianness/SequenceEndianness.cpp + fragmentation/BasicFragmentation.cpp + fragmentation/ArrayFragmentation.cpp + SequenceOverflow.cpp + Alignment.cpp + FullBuffer.cpp + FullBufferCallback.cpp + cross_serialization/FastCDRSerialization.cpp + ) + add_executable(${PROJECT_NAME} ${SRCS}) + set_common_compile_options(${PROJECT_NAME}) + add_gtest(${PROJECT_NAME} + SOURCES + ${SRCS} + DEPENDENCIES + microcdr + fastcdr + ) + target_include_directories(${PROJECT_NAME} PRIVATE ${GTEST_INCLUDE_DIRS} ${FASTCDR_INCLUDE_DIRS}) + target_link_libraries(${PROJECT_NAME} microcdr fastcdr ${GTEST_BOTH_LIBRARIES}) + + set_target_properties(${PROJECT_NAME} PROPERTIES + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED YES + ) +endif() + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/CommonFunctions.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/CommonFunctions.cpp new file mode 100644 index 0000000..dc357ec --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/CommonFunctions.cpp @@ -0,0 +1,208 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#define BUFFER_LENGTH 1024 + +bool operator ==( + const ucdrBuffer& rhs, + const ucdrBuffer& lhs) +{ + return + rhs.init == lhs.init + && rhs.final == lhs.final + && rhs.iterator == lhs.iterator + && rhs.origin == lhs.origin + && rhs.offset == lhs.offset + && rhs.endianness == lhs.endianness + && rhs.last_data_size == lhs.last_data_size + && rhs.error == lhs.error + && rhs.on_full_buffer == lhs.on_full_buffer + && rhs.args == lhs.args; +} + +class CommonFunctions : public ::testing::Test +{ +public: + + void SetUp() override + { + std::memset(buffer, 0, BUFFER_LENGTH); + } + +protected: + + ucdrBuffer ub; + uint8_t buffer[BUFFER_LENGTH]; +}; + +TEST_F(CommonFunctions, ucdr_init_buffer) +{ + ucdr_init_buffer(&ub, buffer, BUFFER_LENGTH); + EXPECT_EQ(ub.init, buffer); + EXPECT_EQ(ub.final, ub.init + BUFFER_LENGTH); + EXPECT_EQ(ub.iterator, ub.init); + EXPECT_EQ(ub.origin, 0); + EXPECT_EQ(ub.offset, ub.origin); + EXPECT_EQ(ub.endianness, UCDR_MACHINE_ENDIANNESS); + EXPECT_EQ(ub.last_data_size, 0); + EXPECT_EQ(ub.error, false); + EXPECT_EQ(ub.on_full_buffer, nullptr); + EXPECT_EQ(ub.args, nullptr); +} + +TEST_F(CommonFunctions, ucdr_init_buffer_origin) +{ + size_t origin = 1; + ucdrBuffer temp_ub; + + ucdr_init_buffer(&ub, buffer, BUFFER_LENGTH); + ub.origin = origin; + ub.offset = ub.origin; + ucdr_init_buffer_origin(&temp_ub, buffer, BUFFER_LENGTH, origin); + EXPECT_EQ(ub, temp_ub); +} + +TEST_F(CommonFunctions, ucdr_init_buffer_origin_offset) +{ + size_t origin = 1; + size_t offset = 1; + ucdrBuffer temp_ub; + + ucdr_init_buffer_origin(&ub, buffer, BUFFER_LENGTH, origin); + ub.iterator += offset; + ub.offset += offset; + ucdr_init_buffer_origin_offset(&temp_ub, buffer, BUFFER_LENGTH, origin, offset); + EXPECT_EQ(ub, temp_ub); +} + +TEST_F(CommonFunctions, ucdr_init_buffer_origin_offset_endian) +{ + size_t origin = 1; + size_t offset = 1; + ucdrEndianness endian = UCDR_BIG_ENDIANNESS; + ucdrBuffer temp_ub; + + ucdr_init_buffer_origin_offset(&ub, buffer, BUFFER_LENGTH, origin, offset); + ub.endianness = endian; + ucdr_init_buffer_origin_offset_endian(&temp_ub, buffer, BUFFER_LENGTH, origin, offset, endian); + EXPECT_EQ(ub, temp_ub); +} + +TEST_F(CommonFunctions, ucdr_copy_buffer) +{ + size_t origin = 1; + size_t offset = 1; + ucdrEndianness endian = UCDR_BIG_ENDIANNESS; + ucdrBuffer temp_ub; + + ucdr_init_buffer_origin_offset_endian(&ub, buffer, BUFFER_LENGTH, origin, offset, endian); + ucdr_copy_buffer(&temp_ub, &ub); + EXPECT_EQ(ub, temp_ub); +} + +TEST_F(CommonFunctions, ucdr_reset_buffer) +{ + ucdrBuffer temp_ub; + + ucdr_init_buffer(&ub, buffer, BUFFER_LENGTH); + ucdr_init_buffer(&temp_ub, buffer, BUFFER_LENGTH); + ucdr_serialize_bool(&temp_ub, true); + ucdr_reset_buffer(&temp_ub); + EXPECT_EQ(ub, temp_ub); +} + +TEST_F(CommonFunctions, ucdr_reset_buffer_offset) +{ + size_t origin = 1; + size_t offset = 1; + ucdrBuffer temp_ub; + + ucdr_init_buffer_origin_offset(&ub, buffer, BUFFER_LENGTH, origin, offset); + ucdr_init_buffer_origin_offset(&temp_ub, buffer, BUFFER_LENGTH, origin, offset); + ucdr_serialize_bool(&temp_ub, true); + ucdr_reset_buffer_offset(&temp_ub, offset); + EXPECT_EQ(ub, temp_ub); +} + +TEST_F(CommonFunctions, ucdr_align) +{ + size_t origin = 1; + size_t offset = 1; + + ucdr_init_buffer_origin_offset(&ub, buffer, BUFFER_LENGTH, origin, offset); + EXPECT_EQ(ucdr_alignment(offset, 1), 0); + EXPECT_EQ(ucdr_buffer_alignment(&ub, 1), 0); + ucdr_align_to(&ub, 1); + EXPECT_EQ(ub.iterator, ub.init + 1); + EXPECT_EQ(ub.offset, ub.origin + 1); + + ucdr_reset_buffer_offset(&ub, offset); + EXPECT_EQ(ucdr_alignment(offset, 2), 1); + EXPECT_EQ(ucdr_buffer_alignment(&ub, 2), 1); + ucdr_align_to(&ub, 2); + EXPECT_EQ(ub.iterator, ub.init + 2); + EXPECT_EQ(ub.offset, ub.origin + 2); + + ucdr_reset_buffer_offset(&ub, offset); + EXPECT_EQ(ucdr_alignment(offset, 4), 3); + EXPECT_EQ(ucdr_buffer_alignment(&ub, 4), 3); + ucdr_align_to(&ub, 4); + EXPECT_EQ(ub.iterator, ub.init + 4); + EXPECT_EQ(ub.offset, ub.origin + 4); + + ucdr_reset_buffer_offset(&ub, offset); + EXPECT_EQ(ucdr_alignment(offset, 8), 7); + EXPECT_EQ(ucdr_buffer_alignment(&ub, 8), 7); + ucdr_align_to(&ub, 8); + EXPECT_EQ(ub.iterator, ub.init + 8); + EXPECT_EQ(ub.offset, ub.origin + 8); +} + +TEST_F(CommonFunctions, ucdr_advance_buffer) +{ + const size_t distance = 7; + + ucdr_init_buffer(&ub, buffer, BUFFER_LENGTH); + ucdr_advance_buffer(&ub, distance); + EXPECT_EQ(ub.iterator, ub.init + distance); + EXPECT_EQ(ub.offset, ub.origin + distance); +} + +TEST_F(CommonFunctions, ucdr_buffer_size) +{ + ucdr_init_buffer(&ub, buffer, BUFFER_LENGTH); + EXPECT_EQ(ucdr_buffer_size(&ub), BUFFER_LENGTH); +} + +TEST_F(CommonFunctions, ucdr_buffer_length) +{ + const size_t distance = 7; + + ucdr_init_buffer(&ub, buffer, BUFFER_LENGTH); + ucdr_advance_buffer(&ub, distance); + EXPECT_EQ(ucdr_buffer_length(&ub), distance); +} + +TEST_F(CommonFunctions, ucdr_buffer_remaining) +{ + const size_t distance = 7; + + ucdr_init_buffer(&ub, buffer, BUFFER_LENGTH); + ucdr_advance_buffer(&ub, distance); + EXPECT_EQ(ucdr_buffer_remaining(&ub), BUFFER_LENGTH - distance); +} \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBuffer.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBuffer.cpp new file mode 100644 index 0000000..2c726e2 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBuffer.cpp @@ -0,0 +1,66 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "FullBuffer.hpp" + +TEST_F(FullBuffer, Block_8) +{ + fill_buffer_except(7); + try_block_8(); +} + +TEST_F(FullBuffer, Block_4) +{ + fill_buffer_except(3); + try_block_4(); +} + +TEST_F(FullBuffer, Block_2) +{ + fill_buffer_except(1); + try_block_2(); +} + +TEST_F(FullBuffer, Block_1) +{ + fill_buffer_except(0); + try_block_1(); +} + +# define SUCCESSFUL_SERIALIZATION 3 +# define ARRAY_SERIALIZATION (SUCCESSFUL_SERIALIZATION + 1) +TEST_F(FullBuffer, ArrayBlock_8) +{ + fill_buffer_except(8 * SUCCESSFUL_SERIALIZATION + 7); + try_array_block_8(ARRAY_SERIALIZATION); +} + +TEST_F(FullBuffer, ArrayBlock_4) +{ + fill_buffer_except(4 * SUCCESSFUL_SERIALIZATION + 3); + try_array_block_4(ARRAY_SERIALIZATION); +} + +TEST_F(FullBuffer, ArrayBlock_2) +{ + fill_buffer_except(2 * SUCCESSFUL_SERIALIZATION + 1); + try_array_block_2(ARRAY_SERIALIZATION); +} + +TEST_F(FullBuffer, ArrayBlock_1) +{ + fill_buffer_except(SUCCESSFUL_SERIALIZATION); + try_array_block_1(ARRAY_SERIALIZATION); +} + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBuffer.hpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBuffer.hpp new file mode 100644 index 0000000..48e09fc --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBuffer.hpp @@ -0,0 +1,132 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "serialization/BasicSerialization.hpp" + +#define ARRAY_CAPACITY 128 + +class FullBuffer : public BasicSerialization +{ +public: + + FullBuffer() + { + } + + void fill_buffer_except( + int gap) + { + for (int i = 0; i < BUFFER_LENGTH - gap; ++i) + { + uint8_t_serialization(); + } + } + + void try_block_1() + { + uint8_t input = 0xAA; + uint8_t output = 0; + + EXPECT_FALSE(ucdr_serialize_uint8_t(&writer, input)); + EXPECT_FALSE(ucdr_deserialize_uint8_t(&reader, &output)); + } + + void try_block_2() + { + uint16_t input = 0xAABB; + uint16_t output = 0; + + EXPECT_FALSE(ucdr_serialize_uint16_t(&writer, input)); + EXPECT_FALSE(ucdr_deserialize_uint16_t(&reader, &output)); + } + + void try_block_4() + { + uint32_t input = 0xAABBCCDD; + uint32_t output = 0; + + EXPECT_FALSE(ucdr_serialize_uint32_t(&writer, input)); + EXPECT_FALSE(ucdr_deserialize_uint32_t(&reader, &output)); + } + + void try_block_8() + { + uint64_t input = 0x0123456789ABCDEF; + uint64_t output = 0; + + EXPECT_FALSE(ucdr_serialize_uint64_t(&writer, input)); + EXPECT_FALSE(ucdr_deserialize_uint64_t(&reader, &output)); + } + + void try_array_block_1( + uint32_t size) + { + uint8_t input[ARRAY_CAPACITY]; + std::fill_n(input, size, uint8_t(0x09)); + uint8_t output[ARRAY_CAPACITY]; + + EXPECT_FALSE(ucdr_serialize_array_uint8_t(&writer, input, size)); + EXPECT_FALSE(ucdr_deserialize_array_uint8_t(&reader, output, size)); + } + + void try_array_block_2( + uint32_t size) + { + uint16_t input[ARRAY_CAPACITY]; + std::fill_n(input, size, int16_t(0x0A0B)); + uint16_t output[ARRAY_CAPACITY]; + + EXPECT_FALSE(ucdr_serialize_array_uint16_t(&writer, input, size)); + EXPECT_FALSE(ucdr_deserialize_array_uint16_t(&reader, output, size)); + } + + void try_array_block_4( + uint32_t size) + { + + uint32_t input[ARRAY_CAPACITY]; + std::fill_n(input, size, 0x0C0D0E0F); + uint32_t output[ARRAY_CAPACITY]; + + EXPECT_FALSE(ucdr_serialize_array_uint32_t(&writer, input, size)); + EXPECT_FALSE(ucdr_deserialize_array_uint32_t(&reader, output, size)); + } + + void try_array_block_8( + uint32_t size) + { + uint64_t input[ARRAY_CAPACITY]; + std::fill_n(input, size, 0x0102030405060708L); + uint64_t output[ARRAY_CAPACITY]; + + EXPECT_FALSE(ucdr_serialize_array_uint64_t(&writer, input, size)); + EXPECT_FALSE(ucdr_deserialize_array_uint64_t(&reader, output, size)); + } + + ~FullBuffer() + { + EXPECT_TRUE(writer.error); + EXPECT_TRUE(reader.error); + + try_block_1(); //a serialization with an error, gives an error + + EXPECT_TRUE(writer.error); + EXPECT_TRUE(reader.error); + + // To satisfy the base destructor + writer.error = false; + reader.error = false; + } + +}; diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBufferCallback.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBufferCallback.cpp new file mode 100644 index 0000000..3617281 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/FullBufferCallback.cpp @@ -0,0 +1,92 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "FullBuffer.hpp" + +class FullBufferCallback : public FullBuffer +{ +public: + + FullBufferCallback() + { + ucdr_set_on_full_buffer_callback(&writer, on_full_buffer, buffer); + ucdr_set_on_full_buffer_callback(&reader, on_full_buffer, buffer); + } + +protected: + + static bool on_full_buffer( + ucdrBuffer* ub, + void* args) + { + uint8_t* buffer = static_cast(args); + + EXPECT_EQ(buffer, ub->init); + EXPECT_EQ(buffer + BUFFER_LENGTH, ub->final); //only satisfied if BUFFER_LENGTH is alignment to 8. + + return true; + } + +}; + +TEST_F(FullBufferCallback, Block_8_callback) +{ + fill_buffer_except(7); + try_block_8(); +} + +TEST_F(FullBufferCallback, Block_4_callback) +{ + fill_buffer_except(3); + try_block_4(); +} + +TEST_F(FullBufferCallback, Block_2_callback) +{ + fill_buffer_except(1); + try_block_2(); +} + +TEST_F(FullBufferCallback, Block_1_callback) +{ + fill_buffer_except(0); + try_block_1(); +} + +# define SUCCESSFUL_SERIALIZATION 3 +# define ARRAY_SERIALIZATION (SUCCESSFUL_SERIALIZATION + 1) +TEST_F(FullBufferCallback, ArrayBlock_8_callback) +{ + fill_buffer_except(8 * SUCCESSFUL_SERIALIZATION + 7); + try_array_block_8(ARRAY_SERIALIZATION); +} + +TEST_F(FullBufferCallback, ArrayBlock_4_callback) +{ + fill_buffer_except(4 * SUCCESSFUL_SERIALIZATION + 3); + try_array_block_4(ARRAY_SERIALIZATION); +} + +TEST_F(FullBufferCallback, ArrayBlock_2_callback) +{ + fill_buffer_except(2 * SUCCESSFUL_SERIALIZATION + 1); + try_array_block_2(ARRAY_SERIALIZATION); +} + +TEST_F(FullBufferCallback, ArrayBlock_1_callback) +{ + fill_buffer_except(SUCCESSFUL_SERIALIZATION); + try_array_block_1(ARRAY_SERIALIZATION); +} + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/SequenceOverflow.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/SequenceOverflow.cpp new file mode 100644 index 0000000..7f994cd --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/SequenceOverflow.cpp @@ -0,0 +1,75 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "serialization/SequenceSerialization.hpp" + +#define SEQUENCE_SIZE_OVERFLOW ARRAY_CAPACITY + 1 + +class SequenceOverflow : public SequenceSerialization +{ +public: + + SequenceOverflow() + { + set_sequence_size(SEQUENCE_SIZE_OVERFLOW); + } + + ~SequenceOverflow() + { + EXPECT_TRUE(reader.error); + + // To satisfy the base destructor + reader.error = false; + reader.iterator = writer.iterator; + } + +private: +}; + +TEST_F(SequenceOverflow, Block1) +{ + uint8_t input[SEQUENCE_SIZE_OVERFLOW]; + uint8_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_uint8_t(&writer, input, SEQUENCE_SIZE_OVERFLOW)); + EXPECT_FALSE(ucdr_deserialize_sequence_uint8_t(&reader, output, ARRAY_CAPACITY, &output_size)); +} + +TEST_F(SequenceOverflow, Block2) +{ + uint16_t input[SEQUENCE_SIZE_OVERFLOW]; + uint16_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_uint16_t(&writer, input, SEQUENCE_SIZE_OVERFLOW)); + EXPECT_FALSE(ucdr_deserialize_sequence_uint16_t(&reader, output, ARRAY_CAPACITY, &output_size)); +} + +TEST_F(SequenceOverflow, Block4) +{ + uint32_t input[SEQUENCE_SIZE_OVERFLOW]; + uint32_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_uint32_t(&writer, input, SEQUENCE_SIZE_OVERFLOW)); + EXPECT_FALSE(ucdr_deserialize_sequence_uint32_t(&reader, output, ARRAY_CAPACITY, &output_size)); +} + +TEST_F(SequenceOverflow, Block8) +{ + uint64_t input[SEQUENCE_SIZE_OVERFLOW]; + uint64_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_uint64_t(&writer, input, SEQUENCE_SIZE_OVERFLOW)); + EXPECT_FALSE(ucdr_deserialize_sequence_uint64_t(&reader, output, ARRAY_CAPACITY, &output_size)); +} + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/case/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/case/CMakeLists.txt new file mode 100644 index 0000000..0ce4983 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/case/CMakeLists.txt @@ -0,0 +1,36 @@ +# Copyright 2019 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +add_test( + NAME + test-case-installation + COMMAND + ${CMAKE_COMMAND} + -DINSTALL_PATH=${CMAKE_INSTALL_PREFIX} + -DLIBRARY_NAME=$ + -P ${CMAKE_CURRENT_SOURCE_DIR}/installation/InstallationTest.cmake + ) + +add_test( + NAME + test-case-packaging + COMMAND + ${CMAKE_COMMAND} + -DORIGINAL_DIR=${CMAKE_CURRENT_SOURCE_DIR}/packaging + -DCMAKE_PREFIX_PATH=${CMAKE_INSTALL_PREFIX} + -DREQUIRED_VERSION=${PROJECT_VERSION} + -DCMAKE_GENERATOR_TOOLSET=${CMAKE_GENERATOR_TOOLSET} + -DCMAKE_GENERATOR_PLATFORM=${CMAKE_GENERATOR_PLATFORM} + -P ${CMAKE_CURRENT_SOURCE_DIR}/packaging/Packaging.cmake + ) \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/cross_serialization/FastCDRSerialization.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/cross_serialization/FastCDRSerialization.cpp new file mode 100644 index 0000000..c3138b2 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/cross_serialization/FastCDRSerialization.cpp @@ -0,0 +1,22 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "FastCDRSerialization.hpp" + +TEST_F(FastCDRSerialization, double_sequence_with_aligment_serialization) +{ + double_sequence_with_aligment_serialization(); +} + + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/cross_serialization/FastCDRSerialization.hpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/cross_serialization/FastCDRSerialization.hpp new file mode 100644 index 0000000..863fd62 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/cross_serialization/FastCDRSerialization.hpp @@ -0,0 +1,79 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _FASTCDR_SERIALIZATION_HPP_ +#define _FASTCDR_SERIALIZATION_HPP_ + +#include +#include + +#include +#include +#include + +#define BUFFER_LENGTH 2014 + +class FastCDRSerialization : public ::testing::Test +{ +public: + + void double_sequence_with_aligment_serialization() + { + using namespace eprosima::fastcdr; + + char buffer_fastcdr[BUFFER_LENGTH] = {0}; + + ucdrBuffer reader_fastcdr; + ucdr_init_buffer(&reader_fastcdr, reinterpret_cast(buffer_fastcdr), BUFFER_LENGTH); + + FastBuffer cdrbuffer(buffer_fastcdr, BUFFER_LENGTH); + Cdr cdr_ser(cdrbuffer); + + // Serialize 2 bytes + const uint8_t octet_array[2] = {0xAA, 0xAA}; + cdr_ser.serializeArray(octet_array, 2); + + uint8_t octet_array_out[2] = {}; + EXPECT_TRUE(ucdr_deserialize_array_uint8_t(&reader_fastcdr, octet_array_out, 2)); + EXPECT_EQ(octet_array[0], octet_array_out[0]); + EXPECT_EQ(octet_array[1], octet_array_out[1]); + + // Serialize 2 doubles = 16 Bytes + 1 Header + const double double_seq[2] = {3.14, 3.14}; + cdr_ser.serializeSequence(double_seq, 2); + + double double_seq_out[2]; + uint32_t double_seq_len; + EXPECT_TRUE(ucdr_deserialize_sequence_double(&reader_fastcdr, double_seq_out, 2, &double_seq_len)); + EXPECT_EQ(double_seq_len, 2u); + + // Serialize 0 doubles = 1 header + std::vector double_vector; + cdr_ser.serializeSequence(double_seq, 0); + + EXPECT_TRUE(ucdr_deserialize_sequence_double(&reader_fastcdr, double_seq_out, 2, &double_seq_len)); + EXPECT_EQ(double_seq_len, 0u); + + // Serialize 2 bytes + cdr_ser.serializeArray(octet_array, 2); + + EXPECT_TRUE(ucdr_deserialize_array_uint8_t(&reader_fastcdr, octet_array_out, 2)); + EXPECT_EQ(octet_array[0], octet_array_out[0]); + EXPECT_EQ(octet_array[1], octet_array_out[1]); + + } + +}; + +#endif //_FASTCDR_SERIALIZATION_HPP_ diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/ArrayEndianness.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/ArrayEndianness.cpp new file mode 100644 index 0000000..3f8fbcb --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/ArrayEndianness.cpp @@ -0,0 +1,132 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../serialization/ArraySerialization.hpp" + +class ArrayEndianness : public ArraySerialization, public ::testing::WithParamInterface +{ +public: + + ArrayEndianness() + { + endianness = GetParam(); + } + + virtual ~ArrayEndianness() + { + } + +protected: + + ucdrEndianness endianness; +}; + +TEST_P(ArrayEndianness, Int16) +{ + int16_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, int16_t(0x0A0B)); + int16_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_endian_array_int16_t(&writer, endianness, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_array_int16_t(&reader, endianness, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); +} + +TEST_P(ArrayEndianness, Uint16) +{ + uint16_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, uint16_t(0x0A0Bu)); + uint16_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_endian_array_uint16_t(&writer, endianness, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_array_uint16_t(&reader, endianness, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); +} + +TEST_P(ArrayEndianness, Int32) +{ + int32_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 0x0C0D0E0F); + int32_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_endian_array_int32_t(&writer, endianness, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_array_int32_t(&reader, endianness, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); +} + +TEST_P(ArrayEndianness, Uint32) +{ + uint32_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 0x0C0D0E0F); + uint32_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_endian_array_uint32_t(&writer, endianness, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_array_uint32_t(&reader, endianness, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); +} + +TEST_P(ArrayEndianness, Int64) +{ + int64_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 0x0102030405060708L); + int64_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_endian_array_int64_t(&writer, endianness, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_array_int64_t(&reader, endianness, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); +} + +TEST_P(ArrayEndianness, Uint64) +{ + uint64_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 0x0102030405060708L); + uint64_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_endian_array_uint64_t(&writer, endianness, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_array_uint64_t(&reader, endianness, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); +} + +TEST_P(ArrayEndianness, Float) +{ + float input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 3.141592653589793238462f); + float output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_endian_array_float(&writer, endianness, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_array_float(&reader, endianness, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); +} + +TEST_P(ArrayEndianness, Double) +{ + double input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 3.141592653589793238462); + double output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_endian_array_double(&writer, endianness, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_array_double(&reader, endianness, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); +} + +INSTANTIATE_TEST_CASE_P(ucdrEndianness, ArrayEndianness, + ::testing::Values(UCDR_LITTLE_ENDIANNESS, UCDR_BIG_ENDIANNESS)); diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/BasicEndianness.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/BasicEndianness.cpp new file mode 100644 index 0000000..b8a51d1 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/BasicEndianness.cpp @@ -0,0 +1,124 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../serialization/BasicSerialization.hpp" + +class BasicEndianness : public BasicSerialization, public ::testing::WithParamInterface +{ +public: + + BasicEndianness() + { + endianness = GetParam(); + } + + virtual ~BasicEndianness() + { + } + +protected: + + ucdrEndianness endianness; +}; + +TEST_P(BasicEndianness, Int16) +{ + int16_t input = 0x0A0B; + int16_t output = 0; + + EXPECT_TRUE(ucdr_serialize_endian_int16_t(&writer, endianness, input)); + EXPECT_TRUE(ucdr_deserialize_endian_int16_t(&reader, endianness, &output)); + + EXPECT_EQ(input, output); +} + +TEST_P(BasicEndianness, Uint16) +{ + uint16_t input = 0x0A0B; + uint16_t output = 0; + + EXPECT_TRUE(ucdr_serialize_endian_uint16_t(&writer, endianness, input)); + EXPECT_TRUE(ucdr_deserialize_endian_uint16_t(&reader, endianness, &output)); + + EXPECT_EQ(input, output); +} + +TEST_P(BasicEndianness, Int32) +{ + int32_t input = 0x0C0D0E0F; + int32_t output = 0; + + EXPECT_TRUE(ucdr_serialize_endian_int32_t(&writer, endianness, input)); + EXPECT_TRUE(ucdr_deserialize_endian_int32_t(&reader, endianness, &output)); + + EXPECT_EQ(input, output); +} + +TEST_P(BasicEndianness, Uint32) +{ + uint32_t input = 0x0C0D0E0F; + uint32_t output = 0; + + EXPECT_TRUE(ucdr_serialize_endian_uint32_t(&writer, endianness, input)); + EXPECT_TRUE(ucdr_deserialize_endian_uint32_t(&reader, endianness, &output)); + + EXPECT_EQ(input, output); +} + +TEST_P(BasicEndianness, Int64) +{ + int64_t input = 0x0102030405060708L; + int64_t output = 0; + + EXPECT_TRUE(ucdr_serialize_endian_int64_t(&writer, endianness, input)); + EXPECT_TRUE(ucdr_deserialize_endian_int64_t(&reader, endianness, &output)); + + EXPECT_EQ(input, output); +} + +TEST_P(BasicEndianness, Uint64) +{ + uint64_t input = 0x0102030405060708L; + uint64_t output = 0; + + EXPECT_TRUE(ucdr_serialize_endian_uint64_t(&writer, endianness, input)); + EXPECT_TRUE(ucdr_deserialize_endian_uint64_t(&reader, endianness, &output)); + + EXPECT_EQ(input, output); +} + +TEST_P(BasicEndianness, Float) +{ + float input = 3.141592653589793238462f; + float output = 0; + + EXPECT_TRUE(ucdr_serialize_endian_float(&writer, endianness, input)); + EXPECT_TRUE(ucdr_deserialize_endian_float(&reader, endianness, &output)); + + EXPECT_EQ(input, output); +} + +TEST_P(BasicEndianness, Double) +{ + double input = 3.141592653589793238462; + double output = 0; + + EXPECT_TRUE(ucdr_serialize_endian_double(&writer, endianness, input)); + EXPECT_TRUE(ucdr_deserialize_endian_double(&reader, endianness, &output)); + + EXPECT_EQ(input, output); +} + +INSTANTIATE_TEST_CASE_P(ucdrEndianness, BasicEndianness, + ::testing::Values(UCDR_LITTLE_ENDIANNESS, UCDR_BIG_ENDIANNESS)); diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/SequenceEndianness.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/SequenceEndianness.cpp new file mode 100644 index 0000000..4664fb8 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/endianness/SequenceEndianness.cpp @@ -0,0 +1,180 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../serialization/SequenceSerialization.hpp" + +class SequenceEndianness : public SequenceSerialization, public ::testing::WithParamInterface +{ +public: + + SequenceEndianness() + { + endianness = GetParam(); + } + + virtual ~SequenceEndianness() + { + } + +protected: + + ucdrEndianness endianness; +}; + +TEST_P(SequenceEndianness, Bool) +{ + bool input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, true); + bool output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_bool(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_bool(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Char) +{ + char input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, 'A'); + char output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_char(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_char(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Int8) +{ + int8_t input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, int8_t(0x09)); + int8_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_int8_t(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_int8_t(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Uint8) +{ + uint8_t input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, uint8_t(0x09)); + uint8_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_uint8_t(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_uint8_t(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Int16) +{ + int16_t input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, int16_t(0x0A0B)); + int16_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_int16_t(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_int16_t(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Uint16) +{ + uint16_t input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, uint16_t(0x0A0B)); + uint16_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_uint16_t(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_uint16_t(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Int32) +{ + int32_t input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, 0x0A0B0C0D); + int32_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_int32_t(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_int32_t(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Uint32) +{ + uint32_t input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, 0x0A0B0C0D); + uint32_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_uint32_t(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_uint32_t(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Int64) +{ + int64_t input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, 0x0102030405060708L); + int64_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_int64_t(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_int64_t(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Uint64) +{ + uint64_t input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, 0x0102030405060708L); + uint64_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_uint64_t(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_uint64_t(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Float) +{ + float input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, 3.141592653589793238462f); + float output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_float(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_float(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +TEST_P(SequenceEndianness, Double) +{ + double input[ARRAY_CAPACITY]; + std::fill_n(input, SEQUENCE_SIZE, 3.141592653589793238462); + double output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_endian_sequence_double(&writer, endianness, input, SEQUENCE_SIZE)); + EXPECT_TRUE(ucdr_deserialize_endian_sequence_double(&reader, endianness, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, SEQUENCE_SIZE)); +} + +INSTANTIATE_TEST_CASE_P(ucdrEndianness, SequenceEndianness, + ::testing::Values(UCDR_LITTLE_ENDIANNESS, UCDR_BIG_ENDIANNESS)); diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/fragmentation/ArrayFragmentation.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/fragmentation/ArrayFragmentation.cpp new file mode 100644 index 0000000..5749b4c --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/fragmentation/ArrayFragmentation.cpp @@ -0,0 +1,156 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../serialization/ArraySerialization.hpp" + +#define OFFSET 16 + +class ArrayFragmentation : public ArraySerialization +{ +public: + + ArrayFragmentation() + { + ucdr_set_on_full_buffer_callback(&writer, on_full_buffer, this); + ucdr_set_on_full_buffer_callback(&reader, on_full_buffer, this); + std::memset(buffer2, 0, BUFFER_LENGTH); + for (int i = 0; i < BUFFER_LENGTH - OFFSET; ++i) + { + uint8_t_serialization(); + } + } + +protected: + + uint8_t buffer2[BUFFER_LENGTH]; + uint8_t asymmetric_buffer1[999]; + uint8_t asymmetric_buffer2[1025]; + + static bool on_full_buffer( + ucdrBuffer* ub, + void* args) + { + ArrayFragmentation* obj = static_cast(args); + + ub->init = obj->buffer2; + ub->iterator = ub->init; + ub->final = ub->init + BUFFER_LENGTH; + + return false; + } + + static bool asymmetric_first_on_full_buffer( + ucdrBuffer* ub, + void* args) + { + ArrayFragmentation* obj = static_cast(args); + + ub->init = obj->asymmetric_buffer2; + ub->iterator = ub->init; + ub->final = ub->init + sizeof(asymmetric_buffer2); + ucdr_set_on_full_buffer_callback(ub, asymmetric_last_on_full_buffer, obj); + return false; + } + + static bool asymmetric_last_on_full_buffer( + ucdrBuffer* ub, + void* args) + { + (void) ub; + (void) args; + return true; + } + +}; + +TEST_F(ArrayFragmentation, Bool) +{ + bool_array_serialization(); +} + +TEST_F(ArrayFragmentation, Char) +{ + char_array_serialization(); +} + +TEST_F(ArrayFragmentation, Int8) +{ + int8_t_array_serialization(); +} + +TEST_F(ArrayFragmentation, Uint8) +{ + uint8_t_array_serialization(); +} + +TEST_F(ArrayFragmentation, Int16) +{ + int16_t_array_serialization(); +} + +TEST_F(ArrayFragmentation, Uint16) +{ + uint16_t_array_serialization(); +} + +TEST_F(ArrayFragmentation, Int32) +{ + int32_t_array_serialization(); +} + +TEST_F(ArrayFragmentation, Uint32) +{ + uint32_t_array_serialization(); +} + +TEST_F(ArrayFragmentation, Int64) +{ + int64_t_array_serialization(); +} + +TEST_F(ArrayFragmentation, Uint64) +{ + uint64_t_array_serialization(); +} + +TEST_F(ArrayFragmentation, Float) +{ + float_array_serialization(); +} + +TEST_F(ArrayFragmentation, Double) +{ + double_array_serialization(); +} + +TEST_F(ArrayFragmentation, AsymmetricFragmentation) +{ + constexpr size_t n_elements = (sizeof(asymmetric_buffer1) + sizeof(asymmetric_buffer2)) / sizeof(double); + + ucdr_init_buffer(&writer, asymmetric_buffer1, sizeof(asymmetric_buffer1)); + ucdr_init_buffer(&reader, asymmetric_buffer1, sizeof(asymmetric_buffer1)); + + double input[n_elements]; + std::fill_n(input, n_elements, 3.141592653589793238462); + double output[n_elements]; + std::memset(output, 0, n_elements * sizeof(double)); + + ucdr_set_on_full_buffer_callback(&writer, asymmetric_first_on_full_buffer, this); + ucdr_set_on_full_buffer_callback(&reader, asymmetric_first_on_full_buffer, this); + + EXPECT_TRUE(ucdr_serialize_array_double(&writer, input, n_elements)); + EXPECT_TRUE(ucdr_deserialize_array_double(&reader, output, n_elements)); + + EXPECT_TRUE(0 == std::memcmp(input, output, n_elements * sizeof(double))); +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/fragmentation/BasicFragmentation.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/fragmentation/BasicFragmentation.cpp new file mode 100644 index 0000000..38c0eee --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/fragmentation/BasicFragmentation.cpp @@ -0,0 +1,109 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../serialization/BasicSerialization.hpp" + +class BasicFragmentation : public BasicSerialization +{ +public: + + BasicFragmentation() + { + ucdr_set_on_full_buffer_callback(&writer, on_full_buffer, this); + ucdr_set_on_full_buffer_callback(&reader, on_full_buffer, this); + std::memset(buffer2, 0, BUFFER_LENGTH); + for (int i = 0; i < BUFFER_LENGTH; ++i) + { + uint8_t_serialization(); + } + } + +protected: + + static bool on_full_buffer( + ucdrBuffer* ub, + void* args) + { + BasicFragmentation* obj = static_cast(args); + + ub->init = obj->buffer2; + ub->iterator = ub->init; + ub->final = ub->init + BUFFER_LENGTH; + + return false; + } + + uint8_t buffer2[BUFFER_LENGTH]; +}; + +TEST_F(BasicFragmentation, Bool) +{ + bool_serialization(); +} + +TEST_F(BasicFragmentation, Char) +{ + char_serialization(); +} + +TEST_F(BasicFragmentation, Int8) +{ + int8_t_serialization(); +} + +TEST_F(BasicFragmentation, Uint8) +{ + uint8_t_serialization(); +} + +TEST_F(BasicFragmentation, Int16) +{ + int16_t_serialization(); +} + +TEST_F(BasicFragmentation, Uint16) +{ + uint16_t_serialization(); +} + +TEST_F(BasicFragmentation, Int32) +{ + int32_t_serialization(); +} + +TEST_F(BasicFragmentation, Uint32) +{ + uint32_t_serialization(); +} + +TEST_F(BasicFragmentation, Int64) +{ + int64_t_serialization(); +} + +TEST_F(BasicFragmentation, Uint64) +{ + uint64_t_serialization(); +} + +TEST_F(BasicFragmentation, Float) +{ + float_serialization(); +} + +TEST_F(BasicFragmentation, Double) +{ + double_serialization(); +} + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/ArraySerialization.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/ArraySerialization.cpp new file mode 100644 index 0000000..b244212 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/ArraySerialization.cpp @@ -0,0 +1,75 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ArraySerialization.hpp" + +TEST_F(ArraySerialization, Bool) +{ + bool_array_serialization(); +} + +TEST_F(ArraySerialization, Char) +{ + char_array_serialization(); +} + +TEST_F(ArraySerialization, Int8) +{ + int8_t_array_serialization(); +} + +TEST_F(ArraySerialization, Uint8) +{ + uint8_t_array_serialization(); +} + +TEST_F(ArraySerialization, Int16) +{ + int16_t_array_serialization(); +} + +TEST_F(ArraySerialization, Uint16) +{ + uint16_t_array_serialization(); +} + +TEST_F(ArraySerialization, Int32) +{ + int32_t_array_serialization(); +} + +TEST_F(ArraySerialization, Uint32) +{ + uint32_t_array_serialization(); +} + +TEST_F(ArraySerialization, Int64) +{ + int64_t_array_serialization(); +} + +TEST_F(ArraySerialization, Uint64) +{ + uint64_t_array_serialization(); +} + +TEST_F(ArraySerialization, Float) +{ + float_array_serialization(); +} + +TEST_F(ArraySerialization, Double) +{ + double_array_serialization(); +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/ArraySerialization.hpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/ArraySerialization.hpp new file mode 100644 index 0000000..2d80a2b --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/ArraySerialization.hpp @@ -0,0 +1,180 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _ARRAY_SERIALIZATION_HPP_ +#define _ARRAY_SERIALIZATION_HPP_ + +#include "BasicSerialization.hpp" + +#define ARRAY_SIZE 32 + +class ArraySerialization : public BasicSerialization +{ +public: + + ArraySerialization() + { + } + + virtual ~ArraySerialization() + { + } + + void bool_array_serialization() + { + bool input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, true); + bool output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_bool(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_bool(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void char_array_serialization() + { + char input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 'A'); + char output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_char(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_char(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void int8_t_array_serialization() + { + int8_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, int8_t(0x09)); + int8_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_int8_t(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_int8_t(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void uint8_t_array_serialization() + { + uint8_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, uint8_t(0x09)); + uint8_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_uint8_t(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_uint8_t(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void int16_t_array_serialization() + { + int16_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, int16_t(0x0A0B)); + int16_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_int16_t(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_int16_t(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void uint16_t_array_serialization() + { + uint16_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, uint16_t(0x0A0B)); + uint16_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_uint16_t(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_uint16_t(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void int32_t_array_serialization() + { + int32_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 0x0C0D0E0F); + int32_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_int32_t(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_int32_t(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void uint32_t_array_serialization() + { + uint32_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 0x0C0D0E0F); + uint32_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_uint32_t(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_uint32_t(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void int64_t_array_serialization() + { + int64_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 0x0102030405060708L); + int64_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_int64_t(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_int64_t(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void uint64_t_array_serialization() + { + uint64_t input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 0x0102030405060708L); + uint64_t output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_uint64_t(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_uint64_t(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void float_array_serialization() + { + float input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 3.141592653589793238462f); + float output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_float(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_float(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + + void double_array_serialization() + { + double input[ARRAY_SIZE]; + std::fill_n(input, ARRAY_SIZE, 3.141592653589793238462); + double output[ARRAY_SIZE]; + + EXPECT_TRUE(ucdr_serialize_array_double(&writer, input, ARRAY_SIZE)); + EXPECT_TRUE(ucdr_deserialize_array_double(&reader, output, ARRAY_SIZE)); + + EXPECT_TRUE(0 == std::memcmp(input, output, ARRAY_SIZE)); + } + +}; + +#endif //_ARRAY_SERIALIZATION_HPP_ \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/BasicSerialization.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/BasicSerialization.cpp new file mode 100644 index 0000000..9618f35 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/BasicSerialization.cpp @@ -0,0 +1,76 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "BasicSerialization.hpp" + +TEST_F(BasicSerialization, Bool) +{ + bool_serialization(); +} + +TEST_F(BasicSerialization, Char) +{ + char_serialization(); +} + +TEST_F(BasicSerialization, Int8) +{ + int8_t_serialization(); +} + +TEST_F(BasicSerialization, Uint8) +{ + uint8_t_serialization(); +} + +TEST_F(BasicSerialization, Int16) +{ + int16_t_serialization(); +} + +TEST_F(BasicSerialization, Uint16) +{ + uint16_t_serialization(); +} + +TEST_F(BasicSerialization, Int32) +{ + int32_t_serialization(); +} + +TEST_F(BasicSerialization, Uint32) +{ + uint32_t_serialization(); +} + +TEST_F(BasicSerialization, Int64) +{ + int64_t_serialization(); +} + +TEST_F(BasicSerialization, Uint64) +{ + uint64_t_serialization(); +} + +TEST_F(BasicSerialization, Float) +{ + float_serialization(); +} + +TEST_F(BasicSerialization, Double) +{ + double_serialization(); +} + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/BasicSerialization.hpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/BasicSerialization.hpp new file mode 100644 index 0000000..1fc24d3 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/BasicSerialization.hpp @@ -0,0 +1,202 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _BASIC_SERIALIZATION_HPP_ +#define _BASIC_SERIALIZATION_HPP_ + +#include +#include +#include +#include + +#define BUFFER_LENGTH 1024 + +class BasicSerialization : public ::testing::Test +{ +public: + + BasicSerialization() + { + std::memset(buffer, 0, BUFFER_LENGTH); + ucdr_init_buffer(&writer, buffer, BUFFER_LENGTH); + ucdr_init_buffer(&reader, buffer, BUFFER_LENGTH); + } + + void check_data_size( + uint32_t data_size) + { + EXPECT_EQ(writer.last_data_size, data_size); + EXPECT_EQ(reader.last_data_size, data_size); + } + + virtual ~BasicSerialization() + { + EXPECT_EQ(writer.iterator, reader.iterator); + EXPECT_FALSE(writer.error); + EXPECT_FALSE(reader.error); + } + + void bool_serialization() + { + bool input = true; + bool output = 0; + + EXPECT_TRUE(ucdr_serialize_bool(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_bool(&reader, &output)); + + EXPECT_EQ(input, output); + + check_data_size(1); + } + + void char_serialization() + { + char input = 'A'; + char output = 0; + + EXPECT_TRUE(ucdr_serialize_char(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_char(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(1); + } + + void int8_t_serialization() + { + int8_t input = 0x09; + int8_t output = 0; + + EXPECT_TRUE(ucdr_serialize_int8_t(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_int8_t(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(1); + } + + void uint8_t_serialization() + { + uint8_t input = 0x09; + uint8_t output = 0; + + EXPECT_TRUE(ucdr_serialize_uint8_t(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_uint8_t(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(1); + } + + void int16_t_serialization() + { + int16_t input = 0x0A0B; + int16_t output = 0; + + EXPECT_TRUE(ucdr_serialize_int16_t(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_int16_t(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(2); + } + + void uint16_t_serialization() + { + uint16_t input = 0x0A0B; + uint16_t output = 0; + + EXPECT_TRUE(ucdr_serialize_uint16_t(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_uint16_t(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(2); + } + + void int32_t_serialization() + { + int32_t input = 0x0C0D0E0F; + int32_t output = 0; + + EXPECT_TRUE(ucdr_serialize_int32_t(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_int32_t(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(4); + } + + void uint32_t_serialization() + { + uint32_t input = 0x0C0D0E0F; + uint32_t output = 0; + + EXPECT_TRUE(ucdr_serialize_uint32_t(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_uint32_t(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(4); + } + + void int64_t_serialization() + { + int64_t input = 0x0102030405060708L; + int64_t output = 0; + + EXPECT_TRUE(ucdr_serialize_int64_t(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_int64_t(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(8); + } + + void uint64_t_serialization() + { + uint64_t input = 0x0102030405060708L; + uint64_t output = 0; + + EXPECT_TRUE(ucdr_serialize_uint64_t(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_uint64_t(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(8); + } + + void float_serialization() + { + float input = 3.141592653589793238462f; + float output = 0; + + EXPECT_TRUE(ucdr_serialize_float(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_float(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(4); + } + + void double_serialization() + { + double input = 3.141592653589793238462; + double output = 0; + + EXPECT_TRUE(ucdr_serialize_double(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_double(&reader, &output)); + + EXPECT_EQ(input, output); + check_data_size(8); + } + +protected: + + ucdrBuffer writer; + ucdrBuffer reader; + uint8_t buffer[BUFFER_LENGTH]; +}; + +#endif //_BASIC_SERIALIZATION_HPP_ diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/SequenceSerialization.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/SequenceSerialization.cpp new file mode 100644 index 0000000..7f15dab --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/SequenceSerialization.cpp @@ -0,0 +1,76 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "SequenceSerialization.hpp" + +TEST_F(SequenceSerialization, Bool) +{ + bool_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Char) +{ + char_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Int8) +{ + int8_t_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Uint8) +{ + uint8_t_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Int16) +{ + int16_t_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Uint16) +{ + uint16_t_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Int32) +{ + int32_t_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Uint32) +{ + uint32_t_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Int64) +{ + int64_t_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Uint64) +{ + uint64_t_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Float) +{ + float_sequence_serialization(); +} + +TEST_F(SequenceSerialization, Double) +{ + double_sequence_serialization(); +} + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/SequenceSerialization.hpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/SequenceSerialization.hpp new file mode 100644 index 0000000..7faa46a --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/SequenceSerialization.hpp @@ -0,0 +1,193 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _SEQUENCE_SERIALIZATION_HPP_ +#define _SEQUENCE_SERIALIZATION_HPP_ + +#include "ArraySerialization.hpp" + +#define SEQUENCE_SIZE 16 +#define ARRAY_CAPACITY ARRAY_SIZE + +class SequenceSerialization : public ArraySerialization +{ +public: + + SequenceSerialization() + : sequence_size(SEQUENCE_SIZE) + { + } + + virtual ~SequenceSerialization() + { + EXPECT_EQ(output_size, sequence_size); + } + + void set_sequence_size( + uint32_t size) + { + sequence_size = size; + } + + void bool_sequence_serialization() + { + bool input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, true); + bool output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_bool(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_bool(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void char_sequence_serialization() + { + char input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, 'A'); + char output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_char(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_char(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void int8_t_sequence_serialization() + { + int8_t input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, int8_t(0x09)); + int8_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_int8_t(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_int8_t(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void uint8_t_sequence_serialization() + { + uint8_t input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, uint8_t(0x09)); + uint8_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_uint8_t(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_uint8_t(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void int16_t_sequence_serialization() + { + int16_t input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, int16_t(0x0A0B)); + int16_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_int16_t(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_int16_t(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void uint16_t_sequence_serialization() + { + uint16_t input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, uint16_t(0x0A0B)); + uint16_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_uint16_t(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_uint16_t(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void int32_t_sequence_serialization() + { + int32_t input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, 0x0A0B0C0D); + int32_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_int32_t(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_int32_t(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void uint32_t_sequence_serialization() + { + uint32_t input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, 0x0A0B0C0D); + uint32_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_uint32_t(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_uint32_t(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void int64_t_sequence_serialization() + { + int64_t input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, 0x0102030405060708L); + int64_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_int64_t(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_int64_t(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void uint64_t_sequence_serialization() + { + uint64_t input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, 0x0102030405060708L); + uint64_t output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_uint64_t(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_uint64_t(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void float_sequence_serialization() + { + float input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, 3.141592653589793238462f); + float output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_float(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_float(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + + void double_sequence_serialization() + { + double input[ARRAY_CAPACITY]; + std::fill_n(input, sequence_size, 3.141592653589793238462); + double output[ARRAY_CAPACITY]; + + EXPECT_TRUE(ucdr_serialize_sequence_double(&writer, input, sequence_size)); + EXPECT_TRUE(ucdr_deserialize_sequence_double(&reader, output, ARRAY_CAPACITY, &output_size)); + + EXPECT_TRUE(0 == std::memcmp(input, output, sequence_size)); + } + +protected: + + uint32_t sequence_size; + uint32_t output_size = 0; +}; + +#endif //_SEQUENCE_SERIALIZATION_HPP_ diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/StringSerialization.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/StringSerialization.cpp new file mode 100644 index 0000000..fa57080 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/StringSerialization.cpp @@ -0,0 +1,21 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "StringSerialization.hpp" + +TEST_F(StringSerialization, String) +{ + string_serialization(); +} + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/StringSerialization.hpp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/StringSerialization.hpp new file mode 100644 index 0000000..5564fd0 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/test/serialization/StringSerialization.hpp @@ -0,0 +1,47 @@ +// Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _STRING_SERIALIZATION_HPP_ +#define _STRING_SERIALIZATION_HPP_ + +#include "BasicSerialization.hpp" + +#define MAX_STRING_LENGTH 64 + +class StringSerialization : public BasicSerialization +{ +public: + + StringSerialization() + { + } + + virtual ~StringSerialization() + { + } + + void string_serialization() + { + char input[MAX_STRING_LENGTH] = "This is a message test"; + char output[MAX_STRING_LENGTH] = {0}; + + EXPECT_TRUE(ucdr_serialize_string(&writer, input)); + EXPECT_TRUE(ucdr_deserialize_string(&reader, output, MAX_STRING_LENGTH)); + + EXPECT_STREQ(input, output); + } + +}; + +#endif //_STRING_SERIALIZATION_HPP_ diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/valgrind.supp b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/valgrind.supp new file mode 100644 index 0000000..b7fb16a --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micro-CDR/valgrind.supp @@ -0,0 +1,17 @@ +{ + + Memcheck:Leak + match-leak-kinds: reachable + fun:*alloc + ... + obj:*ld-* + ... +} +{ + + Memcheck:Leak + fun:malloc + fun:CRYPTO_zalloc + ... + obj:*libcrypto* +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt new file mode 100644 index 0000000..29c442a --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt @@ -0,0 +1,118 @@ +############################################################################ +# +# 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(msg_out_path ${CMAKE_CURRENT_BINARY_DIR}) +get_filename_component(micrortps_bridge_path ${msg_out_path} PATH) + +option(BUILD_MICRORTPS_AGENT "enable building the micrortps_agent after its generation" OFF) + +if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_topics}" STREQUAL "") + set(send_topic_files) + foreach(topic ${config_rtps_send_topics}) + list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) + endforeach() + + set(receive_topic_files) + foreach(topic ${config_rtps_receive_topics}) + list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) + endforeach() + + if (NOT "${config_rtps_send_alias_topics}" STREQUAL "") + set(config_rtps_send_topics "${config_rtps_send_topics};${config_rtps_send_alias_topics}") + endif() + + if (NOT "${config_rtps_receive_alias_topics}" STREQUAL "") + set(config_rtps_receive_topics "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}") + endif() + + foreach(topic ${config_rtps_send_topics}) + list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/${topic}_Publisher.cpp) + list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/${topic}_Publisher.h) + endforeach() + + foreach(topic ${config_rtps_receive_topics}) + list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/${topic}_Subscriber.cpp) + list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/${topic}_Subscriber.h) + endforeach() + + list(APPEND topic_bridge_files_out + ${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp + ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp + ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.h + ) + + add_custom_command(OUTPUT ${topic_bridge_files_out} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/generate_microRTPS_bridge.py + --fastrtpsgen-dir $ENV{FASTRTPSGEN_DIR} + --generate-idl + --mkdir-build + --generate-cmakelists + --topic-msg-dir ${PX4_SOURCE_DIR}/msg + --uorb-templates-dir templates/uorb_microcdr + --urtps-templates-dir templates/urtps + --agent-outdir ${micrortps_bridge_path}/micrortps_agent/src + --client-outdir ${micrortps_bridge_path}/micrortps_client + --idl-dir ${micrortps_bridge_path}/micrortps_agent/idl + >micrortps_bridge.log 2>&1 || cat micrortps_bridge.log + DEPENDS ${send_topic_files} ${receive_topic_files} + COMMENT "Generating RTPS topic bridge" + ) + add_custom_target(topic_bridge_files DEPENDS ${topic_bridge_files_out}) + + px4_add_module( + MODULE modules__micrortps_bridge__micrortps_client + MAIN micrortps_client + STACK_MAIN 4096 + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} + ${micrortps_bridge_path}/micrortps_client + SRCS + microRTPS_client_main.cpp + ${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp + ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp + MODULE_CONFIG + module.yaml + DEPENDS + topic_bridge_files + ) + target_link_libraries(modules__micrortps_bridge__micrortps_client PRIVATE uorb_msgs_microcdr) + + if (BUILD_MICRORTPS_AGENT) + add_custom_command(TARGET modules__micrortps_bridge__micrortps_client POST_BUILD + COMMAND ${PX4_SOURCE_DIR}/Tools/build_micrortps_agent.sh + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + COMMENT "Building micrortps_agent..." + ) + # add_subdirectory(${micrortps_bridge_path}/micrortps_agent) + endif() +endif() diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h new file mode 100644 index 0000000..381b9a1 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * 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 of the copyright holder 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 HOLDER 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 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#define LOOPS -1 +#define SLEEP_US 1000 +#define BAUDRATE 460800 +#define MAX_DATA_RATE 10000000 +#define DEVICE "/dev/ttyACM0" +#define POLL_MS 1 +#define IP "127.0.0.1" +#define DEFAULT_RECV_PORT 2019 +#define DEFAULT_SEND_PORT 2020 +#define MIN_TX_INTERVAL_US 1000.f +#define MAX_TX_INTERVAL_US 1000000.f + + +void *send(void *args); +void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, uint64_t &total_rcvd, + uint64_t &total_sent, uint64_t &sent_last_sec, + uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop); + +struct baudtype { + speed_t code; + uint32_t val; +}; + +struct options { + enum class eTransports { + UART, + UDP + }; + eTransports transport = options::eTransports::UART; + char device[64] = DEVICE; + char ip[16] = IP; + uint16_t recv_port = DEFAULT_RECV_PORT; + uint16_t send_port = DEFAULT_SEND_PORT; + uint32_t sleep_us = SLEEP_US; + uint32_t baudrate = BAUDRATE; + uint32_t datarate = 0; + uint32_t poll_ms = POLL_MS; + int loops = LOOPS; + bool sw_flow_control = false; + bool hw_flow_control = false; + bool verbose_debug = false; +}; + +extern struct options _options; +extern bool _should_exit_task; +extern Transport_node *transport_node; diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp new file mode 100644 index 0000000..6909d50 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -0,0 +1,306 @@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * 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 of the copyright holder 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 HOLDER 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 +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]); + +static int _rtps_task = -1; +bool _should_exit_task = false; +Transport_node *transport_node = nullptr; +struct options _options; + +struct timespec begin; +struct timespec end; + +uint64_t total_rcvd{0}; +uint64_t total_sent{0}; +uint64_t rcvd_last_sec{0}; +uint64_t sent_last_sec{0}; +uint64_t received{0}; +uint64_t sent{0}; +int rcv_loop{0}; +int send_loop{0}; + +static void usage(const char *name) +{ + PRINT_MODULE_USAGE_NAME("micrortps_client", "communication"); + PRINT_MODULE_USAGE_COMMAND("start"); + + PRINT_MODULE_USAGE_PARAM_STRING('t', "UART", "UART|UDP", "Transport protocol", true); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyACM0", "", "Select Serial Device", true); + PRINT_MODULE_USAGE_PARAM_INT('b', 460800, 9600, 3000000, "Baudrate (can also be p:)", true); + PRINT_MODULE_USAGE_PARAM_INT('m', 0, 10, 10000000, "Maximum sending data rate in B/s", true); + PRINT_MODULE_USAGE_PARAM_INT('p', -1, 1, 1000, "Poll timeout for UART in ms", true); + PRINT_MODULE_USAGE_PARAM_INT('l', 10000, -1, 100000, "Limit number of iterations until the program exits (-1=infinite)", + true); + PRINT_MODULE_USAGE_PARAM_INT('w', 1, 1, 1000000, "Time in us for which each read from the link iteration sleeps", true); + PRINT_MODULE_USAGE_PARAM_INT('r', 2019, 0, 65536, "Select UDP Network Port for receiving (local)", true); + PRINT_MODULE_USAGE_PARAM_INT('s', 2020, 0, 65536, "Select UDP Network Port for sending (remote)", true); + PRINT_MODULE_USAGE_PARAM_STRING('i', "127.0.0.1", "", "Select IP address (remote)", true); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Activate UART link SW flow control", true); + PRINT_MODULE_USAGE_PARAM_FLAG('h', "Activate UART link HW flow control", true); + PRINT_MODULE_USAGE_PARAM_FLAG('v', "Add more verbosity", true); + + PRINT_MODULE_USAGE_COMMAND("stop"); + PRINT_MODULE_USAGE_COMMAND("status"); +} + +static int parse_options(int argc, char *argv[]) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "t:d:l:w:b:m:p:r:s:i:fhv", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ? + options::eTransports::UDP + : options::eTransports::UART; break; + + case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; + + case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; + + case 'w': _options.sleep_us = strtoul(myoptarg, nullptr, 10); break; + + case 'b': { + int baudrate = 0; + + if (px4_get_parameter_value(myoptarg, baudrate) != 0) { + PX4_ERR("baudrate parsing failed"); + } + + _options.baudrate = baudrate; + + break; + } + + case 'm': _options.datarate = strtoul(myoptarg, nullptr, 10); break; + + case 'p': _options.poll_ms = strtoul(myoptarg, nullptr, 10); break; + + case 'r': _options.recv_port = strtoul(myoptarg, nullptr, 10); break; + + case 's': _options.send_port = strtoul(myoptarg, nullptr, 10); break; + + case 'i': if (nullptr != myoptarg) strcpy(_options.ip, myoptarg); break; + + case 'f': _options.sw_flow_control = true; break; + + case 'h': _options.hw_flow_control = true; break; + + case 'v': _options.verbose_debug = true; break; + + default: + usage(argv[1]); + return -1; + } + } + + if (_options.datarate > MAX_DATA_RATE) { + _options.datarate = MAX_DATA_RATE; + PX4_WARN("Invalid data rate. Using max datarate of %ul B/s", MAX_DATA_RATE); + } + + if (_options.poll_ms < 1) { + PX4_WARN("Poll timeout too low, using %ul ms", POLL_MS); + } + + if (_options.hw_flow_control && _options.sw_flow_control) { + PX4_ERR("HW and SW flow control set. Please set only one or another"); + return -1; + } + + return 0; +} + +static int micrortps_start(int argc, char *argv[]) +{ + if (0 > parse_options(argc, argv)) { + PX4_INFO("EXITING..."); + _rtps_task = -1; + return -1; + } + + switch (_options.transport) { + case options::eTransports::UART: { + transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms, + _options.sw_flow_control, _options.hw_flow_control, _options.verbose_debug); + PX4_INFO("UART transport: device: %s; baudrate: %" PRIu32 "; sleep: %" PRIu32 "ms; poll: %" PRIu32 + "ms; flow_control: %s", + _options.device, _options.baudrate, _options.sleep_us, _options.poll_ms, + _options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No")); + } + break; + + case options::eTransports::UDP: { + transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port, + _options.verbose_debug); + PX4_INFO("UDP transport: ip address: %s; recv port: %" PRIu16 "; send port: %" PRIu16 "; sleep: %" PRIu32 "us", + _options.ip, _options.recv_port, _options.send_port, _options.sleep_us); + + } + break; + + default: + _rtps_task = -1; + PX4_INFO("EXITING..."); + return -1; + } + + if (0 > transport_node->init()) { + PX4_INFO("EXITING..."); + _rtps_task = -1; + return -1; + } + + micrortps_start_topics(_options.datarate, begin, total_rcvd, total_sent, sent_last_sec, rcvd_last_sec, received, sent, + rcv_loop, + send_loop); + + px4_clock_gettime(CLOCK_REALTIME, &end); + + const double elapsed_secs = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); + + PX4_INFO("RECEIVED: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s", + received, rcv_loop, total_rcvd, elapsed_secs, static_cast(total_rcvd / (1e3 * elapsed_secs))); + PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s", + sent, send_loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs)); + + delete transport_node; + + transport_node = nullptr; + + PX4_INFO("Stopped!"); + + fflush(stdout); + + _rtps_task = -1; + + return 0; +} + +int micrortps_client_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(argv[0]); + return -1; + } + + if (!strcmp(argv[1], "start")) { + if (_rtps_task != -1) { + PX4_INFO("Already running"); + return -1; + } + + _rtps_task = px4_task_spawn_cmd("micrortps_client", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2900, + (px4_main_t) micrortps_start, + (char *const *)argv); + + if (_rtps_task < 0) { + PX4_WARN("Could not start task"); + _rtps_task = -1; + return -1; + } + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (_rtps_task == -1) { + PX4_INFO("Not running"); + + } else { + px4_clock_gettime(CLOCK_REALTIME, &end); + + const double elapsed_secs = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); + + printf("\tup and running for %.03f seconds\n", elapsed_secs); + printf("\tnr. of messages received: %" PRIu64 "\n", received); + printf("\tnr. of messages sent: %" PRIu64 "\n", sent); + printf("\ttotal data read: %" PRIu64 " bytes\n", total_rcvd); + printf("\ttotal data sent: %" PRIu64 " bytes\n", total_sent); + printf("\trates:\n"); + printf("\t rx: %.3f kB/s\n", rcvd_last_sec / 1e3); + printf("\t tx: %.3f kB/s\n", sent_last_sec / 1e3); + printf("\t avg rx: %.3f kB/s\n", total_rcvd / (1e3 * elapsed_secs)); + printf("\t avg tx: %.3f kB/s\n", total_sent / (1e3 * elapsed_secs)); + printf("\t tx rate max:"); + + if (_options.datarate != 0) { + printf(" %.1f kB/s\n", _options.datarate / 1e3); + + } else { + printf(" Unlimited\n"); + } + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (_rtps_task == -1) { + PX4_INFO("Not running"); + return -1; + } + + _should_exit_task = true; + + if (nullptr != transport_node) { transport_node->close(); } + + _rtps_task = -1; + return 0; + } + + usage(argv[0]); + return -1; +} diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/module.yaml b/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/module.yaml new file mode 100644 index 0000000..608c0c3 --- /dev/null +++ b/src/prometheus_px4/src/modules/micrortps_bridge/micrortps_client/module.yaml @@ -0,0 +1,17 @@ +module_name: RTPS +serial_config: + - command: | + protocol_splitter start ${SERIAL_DEV} + mavlink start -d /dev/mavlink -b p:${BAUD_PARAM} -m onboard -r 5000 -x + micrortps_client start -d /dev/rtps -b p:${BAUD_PARAM} -l -1 + port_config_param: + name: RTPS_MAV_CONFIG + group: RTPS + label: MAVLink + FastRTPS + - command: | + micrortps_client start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} -l -1 + port_config_param: + name: RTPS_CONFIG + group: RTPS + label: FastRTPS + diff --git a/src/prometheus_px4/src/modules/micrortps_bridge/res/basic_example_flow.png b/src/prometheus_px4/src/modules/micrortps_bridge/res/basic_example_flow.png new file mode 100644 index 0000000..a48972b Binary files /dev/null and b/src/prometheus_px4/src/modules/micrortps_bridge/res/basic_example_flow.png differ diff --git a/src/prometheus_px4/src/modules/muorb/adsp/CMakeLists.txt b/src/prometheus_px4/src/modules/muorb/adsp/CMakeLists.txt new file mode 100644 index 0000000..de89911 --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/adsp/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# 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_library(modules__muorb__adsp + px4muorb.cpp + uORBFastRpcChannel.cpp +) +target_include_directories(modules__muorb__adsp PRIVATE ${PX4_SOURCE_DIR}/platforms/common/uORB) diff --git a/src/prometheus_px4/src/modules/muorb/adsp/px4muorb.cpp b/src/prometheus_px4/src/modules/muorb/adsp/px4muorb.cpp new file mode 100644 index 0000000..095fab6 --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/adsp/px4muorb.cpp @@ -0,0 +1,295 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 "px4muorb.hpp" +#include "uORBFastRpcChannel.hpp" +#include "uORBManager.hpp" + +#include +#include +#include +#include "uORB.h" +#include +#include +#include + +__BEGIN_DECLS +extern int dspal_main(int argc, char *argv[]); +__END_DECLS + +int px4muorb_orb_initialize() +{ + HAP_power_request(100, 100, 1000); + shmem_info_p = NULL; + + // The uORB Manager needs to be initialized first up, otherwise the instance is nullptr. + uORB::Manager::initialize(); + // Register the fastrpc muorb with uORBManager. + uORB::Manager::get_instance()->set_uorb_communicator( + uORB::FastRpcChannel::GetInstance()); + + // Now continue with the usual dspal startup. + const char *argv[] = { "dspal", "start" }; + int argc = 2; + int rc; + rc = dspal_main(argc, (char **) argv); + + return rc; +} + +int px4muorb_set_absolute_time_offset(int32_t time_diff_us) +{ + return hrt_set_absolute_time_offset(time_diff_us); +} + +int px4muorb_get_absolute_time(uint64_t *time_us) +{ + *time_us = hrt_absolute_time(); + return 0; +} + +/*update value and param's change bit in shared memory*/ +int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, + int data_len_in_bytes) +{ + unsigned int byte_changed, bit_changed; + union param_value_u *param_value = (union param_value_u *) value; + + if (!shmem_info_p) { + init_shared_memory(); + } + + if (get_shmem_lock(__FILE__, __LINE__) != 0) { + PX4_INFO("Could not get shmem lock\n"); + return -1; + } + + shmem_info_p->params_val[param] = *param_value; + + byte_changed = param / 8; + bit_changed = 1 << param % 8; + shmem_info_p->krait_changed_index[byte_changed] |= bit_changed; + + release_shmem_lock(__FILE__, __LINE__); + + return 0; +} + +int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) +{ + if (!shmem_info_p) { + return -1; + } + + if (get_shmem_lock(__FILE__, __LINE__) != 0) { + PX4_INFO("Could not get shmem lock\n"); + return -1; + } + + for (int i = 0; i < data_len_in_bytes; i++) { + data[i] = shmem_info_p->adsp_changed_index[i]; + } + + release_shmem_lock(__FILE__, __LINE__); + + return 0; +} + +int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, + int data_len_in_bytes) +{ + unsigned int byte_changed, bit_changed; + union param_value_u *param_value = (union param_value_u *) value; + + if (!shmem_info_p) { + return -1; + } + + if (get_shmem_lock(__FILE__, __LINE__) != 0) { + PX4_INFO("Could not get shmem lock\n"); + return -1; + } + + *param_value = shmem_info_p->params_val[param]; + + /*also clear the index since we are holding the lock*/ + byte_changed = param / 8; + bit_changed = 1 << param % 8; + shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed; + + release_shmem_lock(__FILE__, __LINE__); + + return 0; +} + +int px4muorb_topic_advertised(const char *topic_name) +{ + int rc = 0; + PX4_INFO("TEST px4muorb_topic_advertised of [%s] on remote side...", topic_name); + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_remote_topic(topic_name, 1); + + } else { + rc = -1; + } + + return rc; +} + +int px4muorb_topic_unadvertised(const char *topic_name) +{ + int rc = 0; + PX4_INFO("TEST px4muorb_topic_unadvertised of [%s] on remote side...", topic_name); + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_remote_topic(topic_name, 0); + + } else { + rc = -1; + } + + return rc; +} + +int px4muorb_add_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->AddRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_add_subscription(name, 0); + + if (rc != OK) { + channel->RemoveRemoteSubscriber(name); + } + + } else { + rc = -1; + } + + return rc; +} + +int px4muorb_remove_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->RemoveRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_remove_subscription(name); + + } else { + rc = -1; + } + + return rc; +} + +int px4muorb_send_topic_data(const char *name, const uint8_t *data, + int data_len_in_bytes) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_received_message(name, data_len_in_bytes, + (uint8_t *) data); + + } else { + rc = -1; + } + + return rc; +} + +int px4muorb_is_subscriber_present(const char *topic_name, int *status) +{ + int rc = 0; + int32_t local_status = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->is_subscriber_present(topic_name, &local_status); + + if (rc == 0) { + *status = (int) local_status; + } + + return rc; +} + +int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, + uint8_t *data, int data_len_in_bytes, int *bytes_returned) +{ + int rc = 0; + int32_t local_msg_type = 0; + int32_t local_bytes_returned = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); + rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, + data_len_in_bytes, &local_bytes_returned); + *msg_type = (int) local_msg_type; + *bytes_returned = (int) local_bytes_returned; + return rc; +} + +int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, + int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count) +{ + int rc = 0; + int32_t local_bytes_returned = 0; + int32_t local_topic_count = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + //PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data ); + rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes, + &local_bytes_returned, &local_topic_count); + *returned_length_in_bytes = (int) local_bytes_returned; + *topic_count = (int) local_topic_count; + return rc; +} + +int px4muorb_unblock_recieve_msg(void) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->unblock_get_data_method(); + return rc; +} diff --git a/src/prometheus_px4/src/modules/muorb/adsp/px4muorb.hpp b/src/prometheus_px4/src/modules/muorb/adsp/px4muorb.hpp new file mode 100644 index 0000000..e9f039f --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/adsp/px4muorb.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 + +extern "C" { + + int px4muorb_orb_initialize() __EXPORT; + + int px4muorb_set_absolute_time_offset(int32_t time_diff_us) __EXPORT; + + int px4muorb_get_absolute_time(uint64_t *time_us) __EXPORT; + + int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT; + + int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) __EXPORT; + + int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT; + + int px4muorb_topic_advertised(const char *name) __EXPORT; + + int px4muorb_topic_unadvertised(const char *name) __EXPORT; + + int px4muorb_add_subscriber(const char *name) __EXPORT; + + int px4muorb_remove_subscriber(const char *name) __EXPORT; + + int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + + int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT; + + int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) __EXPORT; + + int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes, + int *length_in_bytes, int *topic_count) __EXPORT; + + int px4muorb_unblock_recieve_msg(void) __EXPORT; + +} diff --git a/src/prometheus_px4/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/prometheus_px4/src/modules/muorb/adsp/uORBFastRpcChannel.cpp new file mode 100644 index 0000000..4e0f07a --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -0,0 +1,715 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 "uORBFastRpcChannel.hpp" +#include +#include +#include +#include + +// static initialization. +uORB::FastRpcChannel uORB::FastRpcChannel::_Instance; + +static unsigned long _dropped_pkts; +static unsigned long _get_min = 0xFFFFFF; +static unsigned long _get_max = 0; +static unsigned long _min_q = 200; +static unsigned long _max_q = 0; +static unsigned long _avg_q = 0; +static unsigned long _count = 0; +static unsigned long _get_bulk_min = 0xFFFFFF; +static unsigned long _get_bulk_max = 0; +static unsigned long _bulk_topic_count_min = 0xFFFFFF; +static unsigned long _bulk_topic_count_max = 0; + +//============================================================================== +//============================================================================== +uORB::FastRpcChannel::FastRpcChannel() + : _RxHandler(0) + , _DataQInIndex(0) + , _DataQOutIndex(0) + , _ControlQInIndex(0) + , _ControlQOutIndex(0) +{ + for (int32_t i = 0; i < _MAX_MSG_QUEUE_SIZE; ++ i) { + _DataMsgQueue[i]._MaxBufferSize = 0; + _DataMsgQueue[i]._Length = 0; + _DataMsgQueue[i]._Buffer = 0; + } + + _RemoteSubscribers.clear(); +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::topic_advertised(const char *messageName) +{ + return control_msg_queue_add(_CONTROL_MSG_TYPE_ADVERTISE, messageName); +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::topic_unadvertised(const char *messageName) +{ + return control_msg_queue_add(_CONTROL_MSG_TYPE_UNADVERTISE, messageName); +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::control_msg_queue_add(int32_t msgtype, const char *messageName) +{ + int16_t rc = 0; + hrt_abstime t1, t2; + static hrt_abstime check_time = 0; + + t1 = hrt_absolute_time(); + _QueueMutex.lock(); + bool overwriteData = false; + + if (IsControlQFull()) { + // queue is full. Overwrite the oldest data. + _ControlQOutIndex++; + + if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _ControlQOutIndex = 0; + } + + overwriteData = true; + _dropped_pkts++; + } + + _ControlMsgQueue[ _ControlQInIndex ]._Type = msgtype; + _ControlMsgQueue[ _ControlQInIndex ]._MsgName = messageName; + + _ControlQInIndex++; + + if (_ControlQInIndex == _MAX_MSG_QUEUE_SIZE) { + _ControlQInIndex = 0; + } + + // the assumption here is that each caller reads only one data from either control or data queue. + if (ControlQSize() == 1 && DataQSize() == 0) { // post it only of the queue moves from empty to available. + _DataAvailableSemaphore.post(); + } + + if ((unsigned long)ControlQSize() < _min_q) { _min_q = (unsigned long)ControlQSize(); } + + if ((unsigned long)ControlQSize() > _max_q) { _max_q = (unsigned long)ControlQSize(); } + + _count++; + _avg_q = ((double)((_avg_q * (_count - 1)) + (unsigned long)(ControlQSize()))) / (double)(_count); + + _QueueMutex.unlock(); + t2 = hrt_absolute_time(); + + if ((unsigned long)(t2 - check_time) > 10000000) { + //PX4_DEBUG("MsgName: %20s, t1: %lu, t2: %lu, dt: %lu",messageName, (unsigned long) t1, (unsigned long) t2, (unsigned long) (t2-t1)); + //PX4_DEBUG("Q. Stats: min: %lu, max : %lu, avg: %lu count: %lu ", _min_q, _max_q, (unsigned long)(_avg_q * 1000.0), _count); + _min_q = _MAX_MSG_QUEUE_SIZE * 2; + _max_q = 0; + _avg_q = 0; + _count = 0; + check_time = t2; + } + + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + _Subscribers.push_back(messageName); + PX4_DEBUG("Adding message[%s] to subscriber queue...", messageName); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + _Subscribers.remove(messageName); + + return rc; +} + +int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status) +{ + int16_t rc = 0; + + if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) { + *status = 1; + //PX4_DEBUG("******* Found subscriber for message[%s]....", messageName); + + } else { + *status = 0; + //PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size()); + int i = 0; + + for (std::list::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) { + if (*it == messageName) { + PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i); + } + + ++i; + } + } + + return rc; +} + +int16_t uORB::FastRpcChannel::unblock_get_data_method() +{ + PX4_DEBUG("[unblock_get_data_method] calling post method for _DataAvailableSemaphore()"); + _DataAvailableSemaphore.post(); + return 0; +} +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + hrt_abstime t1, t2; + static hrt_abstime check_time = 0; + + if (_RemoteSubscribers.find(messageName) == _RemoteSubscribers.end()) { + //there is no-remote subscriber. So do not queue the message. + return rc; + } + + t1 = hrt_absolute_time(); + _QueueMutex.lock(); + bool overwriteData = false; + + if (IsDataQFull()) { + // queue is full. Overwrite the oldest data. + //PX4_WARN("[send_message] Queue Full Overwrite the oldest data. in[%ld] out[%ld] max[%ld]", + // _DataQInIndex, _DataQOutIndex, _MAX_MSG_QUEUE_SIZE); + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + overwriteData = true; + _dropped_pkts++; + } + + // now check to see if the data queue's buffer size if large enough to memcpy the data. + // if not, delete the old buffer and re-create a new buffer of larger size. + check_and_expand_data_buffer(_DataQInIndex, length); + + // now memcpy the data to the buffer. + memcpy(_DataMsgQueue[ _DataQInIndex ]._Buffer, data, length); + _DataMsgQueue[ _DataQInIndex ]._Length = length; + _DataMsgQueue[ _DataQInIndex ]._MsgName = messageName; + + _DataQInIndex++; + + if (_DataQInIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQInIndex = 0; + } + + // the assumption here is that each caller reads only one data from either control or data queue. + //if (!overwriteData) { + if (DataQSize() == 1 && ControlQSize() == 0) { // post it only of the queue moves from empty to available. + _DataAvailableSemaphore.post(); + } + + if ((unsigned long)DataQSize() < _min_q) { _min_q = (unsigned long)DataQSize(); } + + if ((unsigned long)DataQSize() > _max_q) { _max_q = (unsigned long)DataQSize(); } + + _count++; + _avg_q = ((double)((_avg_q * (_count - 1)) + (unsigned long)(DataQSize()))) / (double)(_count); + + _QueueMutex.unlock(); + t2 = hrt_absolute_time(); + + if ((unsigned long)(t2 - check_time) > 10000000) { + //PX4_DEBUG("MsgName: %20s, t1: %lu, t2: %lu, dt: %lu",messageName, (unsigned long) t1, (unsigned long) t2, (unsigned long) (t2-t1)); + //PX4_DEBUG("Q. Stats: min: %lu, max : %lu, avg: %lu count: %lu ", _min_q, _max_q, (unsigned long)(_avg_q * 1000.0), _count); + _min_q = _MAX_MSG_QUEUE_SIZE * 2; + _max_q = 0; + _avg_q = 0; + _count = 0; + check_time = t2; + } + + return rc; +} + +//============================================================================== +//============================================================================== +void uORB::FastRpcChannel::check_and_expand_data_buffer(int32_t index, int32_t length) +{ + if (_DataMsgQueue[ index ]._MaxBufferSize < length) { + // create a new buffer of size length and delete old buffer. + if (_DataMsgQueue[ index ]._Buffer != 0) { + delete _DataMsgQueue[ index ]._Buffer; + } + + _DataMsgQueue[ index ]._Buffer = new uint8_t[ length ]; + + if (_DataMsgQueue[ index ]._Buffer == 0) { + PX4_ERR("Error[check_and_expand_data_buffer] Failed to allocate data queue buffer of size[%ld]", length); + _DataMsgQueue[ index ]._MaxBufferSize = 0; + return; + } + + _DataMsgQueue[ index ]._MaxBufferSize = length; + } +} + +int32_t uORB::FastRpcChannel::DataQSize() +{ + int32_t rc; + rc = (_DataQInIndex - _DataQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +int32_t uORB::FastRpcChannel::ControlQSize() +{ + int32_t rc; + rc = (_ControlQInIndex - _ControlQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +bool uORB::FastRpcChannel::IsControlQFull() +{ + return (ControlQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsControlQEmpty() +{ + return (ControlQSize() == 0); +} + +bool uORB::FastRpcChannel::IsDataQFull() +{ + return (DataQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsDataQEmpty() +{ + return (DataQSize() == 0); +} + +int16_t uORB::FastRpcChannel::get_data +( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned +) +{ + int16_t rc = 0; + PX4_DEBUG("Get data should not be called..."); + return -1; + // wait for data availability + static hrt_abstime check_time = 0; + hrt_abstime t1 = hrt_absolute_time(); + _DataAvailableSemaphore.wait(); + // hrt_abstime t2 = hrt_absolute_time(); + _QueueMutex.lock(); + + if (DataQSize() != 0 || ControlQSize() != 0) { + if (ControlQSize() > 0) { + // read the first element of the Control Queue. + *msg_type = _ControlMsgQueue[ _ControlQOutIndex ]._Type; + + if ((int)_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() < (int)topic_name_len) { + memcpy + ( + topic_name, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.c_str(), + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + + topic_name[_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = 0; + + _ControlQOutIndex++; + + if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _ControlQOutIndex = 0; + } + + } else { + PX4_ERR("Error[get_data-CONTROL]: max topic_name_len[%ld] < controlMsgLen[%d]", + topic_name_len, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + rc = -1; + } + + } else { + // read the first element of the Control Queue. + *msg_type = _DATA_MSG_TYPE; + + if (((int)_DataMsgQueue[ _DataQOutIndex ]._MsgName.size() < topic_name_len) || + (_DataMsgQueue[ _DataQOutIndex ]._Length < data_len_in_bytes)) { + memcpy + ( + topic_name, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.c_str(), + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + + topic_name[_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = _DataMsgQueue[ _DataQOutIndex ]._Length; + memcpy(data, _DataMsgQueue[ _DataQOutIndex ]._Buffer, _DataMsgQueue[ _DataQOutIndex ]._Length); + + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + } else { + PX4_ERR("Error:[get_data-DATA] type msg max topic_name_len[%ld] > dataMsgLen[%d] ", + topic_name_len, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + PX4_ERR("Error:[get_data-DATA] Or data_buffer_len[%ld] > message_size[%ld] ", + data_len_in_bytes, + _DataMsgQueue[ _DataQOutIndex ]._Length + ); + + rc = -1; + } + } + + } else { + PX4_ERR("[get_data] Error: Semaphore is up when there is no data on the control/data queues"); + rc = -1; + } + + _QueueMutex.unlock(); + hrt_abstime t3 = hrt_absolute_time(); + + if ((unsigned long)(t3 - t1) > _get_max) { _get_max = (unsigned long)(t3 - t1); } + + if ((unsigned long)(t3 - t1) < _get_min) { _get_min = (unsigned long)(t3 - t1); } + + if ((unsigned long)(t3 - check_time) > 1000000) { + if (rc != 0) { + topic_name[0] = '\0'; + } + + /* + PX4_DEBUG("GetData: %30s: t1: %lu t2: %lu t3: %lu", topic_name, (unsigned long)t1, (unsigned long)t2, + (unsigned long)t3); + PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); + PX4_DEBUG("ADSP RPC Stats: _get_min: %lu _get_max: %lu _dropped_pkts: %lu", _get_min, _get_max, _dropped_pkts); + */ + check_time = t3; + } + + return rc; +} + + +int16_t uORB::FastRpcChannel::get_bulk_data +( + uint8_t *buffer, + int32_t max_buffer_in_bytes, + int32_t *returned_bytes, + int32_t *topic_count +) +{ + int16_t rc = 0; + // wait for data availability + static hrt_abstime check_time = 0; + hrt_abstime t1 = hrt_absolute_time(); + _DataAvailableSemaphore.wait(); + //hrt_abstime t2 = hrt_absolute_time(); + + _QueueMutex.lock(); + + int32_t bytes_copied = 0; + int32_t copy_result = 0; + *returned_bytes = 0; + *topic_count = 0; + int32_t topic_count_to_return = 0; + + if (DataQSize() != 0 || ControlQSize() != 0) { + if (DataQSize() != 0) { + //PX4_DEBUG( "get_bulk_data: QSize: %d", DataQSize() ); + topic_count_to_return = DataQSize(); + + while (DataQSize() != 0) { + // this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait. + //_DataAvailableSemaphore.wait(); + if (get_msg_size_at(true, _DataQOutIndex) < (max_buffer_in_bytes - bytes_copied)) { + // there is enough space in the buffer, copy the data. + //PX4_DEBUG( "Coping Data to buffer..." ); + copy_result = copy_msg_to_buffer(true, _DataQOutIndex, buffer, bytes_copied, max_buffer_in_bytes); + + if (copy_result == -1) { + if (bytes_copied == 0) { + rc = -1; + } + + break; + + } else { + //PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\ + // buffer[bytes_copied], \ + // buffer[bytes_copied+1], \ + // buffer[bytes_copied+2], \ + // buffer[bytes_copied+3] ); + bytes_copied += copy_result; + (*topic_count)++; + *returned_bytes = bytes_copied; + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + } + + } else { + if (bytes_copied == 0) { + rc = -1; + PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned"); + + } else { + PX4_DEBUG("Exiting out of the while loop..."); + } + + break; + } + } + } + + if (ControlQSize() != 0) { + //PX4_DEBUG( "get_bulk_data: QSize: %d", ControlQSize() ); + topic_count_to_return += ControlQSize(); + + while (ControlQSize() != 0) { + // this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait. + //_DataAvailableSemaphore.wait(); + if (get_msg_size_at(false, _ControlQOutIndex) < (max_buffer_in_bytes - bytes_copied)) { + // there is enough space in the buffer, copy the data. + //PX4_DEBUG( "Coping Control msg to buffer..." ); + copy_result = copy_msg_to_buffer(false, _ControlQOutIndex, buffer, bytes_copied, max_buffer_in_bytes); + + if (copy_result == -1) { + if (bytes_copied == 0) { + rc = -1; + } + + break; + + } else { + //PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\ + // buffer[bytes_copied], \ + // buffer[bytes_copied+1], \ + // buffer[bytes_copied+2], \ + // buffer[bytes_copied+3] ); + bytes_copied += copy_result; + (*topic_count)++; + *returned_bytes = bytes_copied; + _ControlQOutIndex++; + + if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _ControlQOutIndex = 0; + } + } + + } else { + if (bytes_copied == 0) { + rc = -1; + PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned"); + + } else { + PX4_DEBUG("Exiting out of the while loop..."); + } + + break; + } + } + } + + } else { + PX4_ERR("[get_data_bulk] Error: Semaphore is up when there is no data on the control/data queues"); + rc = -1; + } + + if (topic_count_to_return != *topic_count) { + PX4_WARN("Not sending all topics: topics_to_return:[%ld] topics_returning:[%ld]", topic_count_to_return, *topic_count); + } + + _QueueMutex.unlock(); + hrt_abstime t3 = hrt_absolute_time(); + + if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); } + + if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); } + + if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); } + + if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); } + + if ((unsigned long)(t3 - check_time) > 10000000) { + //PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3); + //PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); + //PX4_DEBUG("ADSP RPC Stats: _get_bulk_min: %lu _get_bulk_max: %lu _dropped_pkts: %lu", _get_bulk_min, _get_bulk_max, + // _dropped_pkts); + //PX4_DEBUG(" .... topic_count_min: %lu topic_count_max: %lu", _bulk_topic_count_min, _bulk_topic_count_max); + _get_bulk_max = 0; + _get_bulk_min = 0xFFFFFF; + _bulk_topic_count_min = 0xFFFFFF; + _bulk_topic_count_max = 0; + check_time = t3; + } + + //PX4_DEBUG( "Returning topics: %d bytes_returned: %d", *topic_count, *returned_bytes ); + return rc; +} + +int32_t uORB::FastRpcChannel::get_msg_size_at(bool isData, int32_t index) +{ + // the assumption here is that this is called within the context of semaphore, + // hence lock/unlock is not needed. + int32_t rc = 0; + + if (isData) { + rc += _DataMsgQueue[ index ]._Length; + rc += _DataMsgQueue[ index ]._MsgName.size() + 1; + + } else { + rc += _ControlMsgQueue[ index ]._MsgName.size() + 1; + } + + rc += _PACKET_HEADER_SIZE; + return rc; +} + +int32_t uORB::FastRpcChannel::copy_msg_to_buffer(bool isData, int32_t src_index, uint8_t *dst_buffer, int32_t offset, + int32_t dst_buffer_len) +{ + int32_t rc = -1; + + // before calling this method the following are assumed: + // * sem_lock is acquired for data protection + // * the dst_buffer is validated to + + uint16_t msg_size = (isData ? + (uint16_t)(_DataMsgQueue[ src_index ]._MsgName.size()) : + (uint16_t)(_ControlMsgQueue[ src_index ]._MsgName.size())); + + // compute the different offsets to pack the packets. + int32_t field_header_offset = offset; + int32_t field_topic_name_offset = field_header_offset + sizeof(struct BulkTransferHeader); + int32_t field_data_offset = field_topic_name_offset + msg_size + 1; + + int16_t msg_type = isData ? _DATA_MSG_TYPE : _ControlMsgQueue[ src_index ]._Type; + + struct BulkTransferHeader header = { (uint16_t)msg_type, (uint16_t)(msg_size + 1), + (uint16_t)(isData ? (_DataMsgQueue[ src_index ]._Length) : 0) + }; + + + //PX4_DEBUG( "Offsets: header[%d] name[%d] data[%d]", + // field_header_offset, + // field_topic_name_offset, + // field_data_offset ); + + if (isData && (field_data_offset + _DataMsgQueue[ src_index ]._Length) < dst_buffer_len) { + memmove(&(dst_buffer[field_header_offset]), (char *)(&header), sizeof(header)); + // pack the data here. + memmove + ( + &(dst_buffer[field_topic_name_offset]), + _DataMsgQueue[ src_index ]._MsgName.c_str(), + _DataMsgQueue[ src_index ]._MsgName.size() + ); + + if (_DataMsgQueue[ src_index ]._MsgName.size() == 0) { + PX4_WARN("########## Error MsgName cannot be zero: "); + } + + dst_buffer[ field_topic_name_offset + _DataMsgQueue[ src_index ]._MsgName.size()] = '\0'; + memmove(&(dst_buffer[field_data_offset]), _DataMsgQueue[ src_index ]._Buffer, _DataMsgQueue[ src_index ]._Length); + rc = field_data_offset + _DataMsgQueue[ src_index ]._Length - offset; + + } else if (field_data_offset < dst_buffer_len) { //This is a control message + memmove(&(dst_buffer[field_header_offset]), (char *)(&header), sizeof(header)); + // pack the data here. + memmove + ( + &(dst_buffer[field_topic_name_offset]), + _ControlMsgQueue[ src_index ]._MsgName.c_str(), + _ControlMsgQueue[ src_index ]._MsgName.size() + ); + + if (_ControlMsgQueue[ src_index ]._MsgName.size() == 0) { + PX4_WARN("########## Error MsgName cannot be zero: "); + } + + dst_buffer[ field_topic_name_offset + _ControlMsgQueue[ src_index ]._MsgName.size()] = '\0'; + rc = field_data_offset - offset; + + } else { + PX4_WARN("Error coping the Msg to dst buffer, insuffienct space. "); + + if (isData) { + PX4_WARN("Data... offset[%ld] len[%ld] data_msg_len[%ld]", + offset, dst_buffer_len, (field_data_offset - offset) + _DataMsgQueue[ src_index ]._Length); + + } else { + PX4_WARN("ControlMsg... offset[%ld] len[%ld]", + offset, dst_buffer_len, (field_data_offset - offset)); + } + } + + return rc; +} diff --git a/src/prometheus_px4/src/modules/muorb/adsp/uORBFastRpcChannel.hpp b/src/prometheus_px4/src/modules/muorb/adsp/uORBFastRpcChannel.hpp new file mode 100644 index 0000000..c83b631 --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/adsp/uORBFastRpcChannel.hpp @@ -0,0 +1,301 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ +#ifndef _uORBFastRpcChannel_hpp_ +#define _uORBFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include +#include +#include + +namespace uORB +{ +class FastRpcChannel; +} + +class uORB::FastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::FastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of a topic being advertised. + * + * @param messageName + * This represents the uORB message name(aka topic); This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t topic_advertised(const char *messageName); + + /** + * @brief Interface to notify the remote entity of a topic being unadvertised + * and is no longer publishing messages. + * + * @param messageName + * This represents the uORB message name(aka topic); This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t topic_unadvertised(const char *messageName); + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + //Function to return the data to krait. + int16_t get_data + ( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned + ); + + int16_t get_bulk_data(uint8_t *buffer, int32_t max_size_in_bytes, int32_t *returned_bytes, int32_t *topic_count); + + // function to check if there are subscribers for a topic on adsp. + int16_t is_subscriber_present(const char *messageName, int32_t *status); + + // function to release the blocking semaphore for get_data method. + int16_t unblock_get_data_method(); + + uORBCommunicator::IChannelRxHandler *GetRxHandler() + { + return _RxHandler; + } + + void AddRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.insert(messageName); + } + void RemoveRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.erase(messageName); + } + +private: // data members + static uORB::FastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + + /// data structure to store the messages to be retrived by Krait. + static const int32_t _MAX_MSG_QUEUE_SIZE = 100; + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + static const int32_t _CONTROL_MSG_TYPE_ADVERTISE = 4; + static const int32_t _CONTROL_MSG_TYPE_UNADVERTISE = 5; + + static const int32_t _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES = 2; + static const int32_t _PACKET_FIELD_DATA_LEN_IN_BYTES = 2; + static const int32_t _PACKET_HEADER_SIZE = 1 + //first byte is the MSG Type + _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES + _PACKET_FIELD_DATA_LEN_IN_BYTES; + + struct FastRpcDataMsg { + int32_t _MaxBufferSize; + int32_t _Length; + uint8_t *_Buffer; + std::string _MsgName; + }; + + struct FastRpcControlMsg { + int32_t _Type; + std::string _MsgName; + }; + + struct BulkTransferHeader { + uint16_t _MsgType; + uint16_t _MsgNameLen; + uint16_t _DataLen; + }; + + + struct FastRpcDataMsg _DataMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _DataQInIndex; + int32_t _DataQOutIndex; + + struct FastRpcControlMsg _ControlMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _ControlQInIndex; + int32_t _ControlQOutIndex; + + std::list _Subscribers; + + //utility classes + class Mutex + { + public: + Mutex() + { + sem_init(&_Sem, 0, 1); + } + ~Mutex() + { + sem_destroy(&_Sem); + } + void lock() + { + sem_wait(&_Sem); + } + void unlock() + { + sem_post(&_Sem); + } + private: + sem_t _Sem; + + Mutex(const Mutex &); + + Mutex &operator=(const Mutex &); + }; + + class Semaphore + { + public: + Semaphore() + { + sem_init(&_Sem, 0, 0); + /* _Sem use case is a signal */ + px4_sem_setprotocol(&_Sem, SEM_PRIO_NONE); + } + ~Semaphore() + { + sem_destroy(&_Sem); + } + void post() + { + sem_post(&_Sem); + } + void wait() + { + sem_wait(&_Sem); + } + private: + sem_t _Sem; + Semaphore(const Semaphore &); + Semaphore &operator=(const Semaphore &); + + }; + + Mutex _QueueMutex; + Semaphore _DataAvailableSemaphore; + +private://class members. + /// constructor. + FastRpcChannel(); + + void check_and_expand_data_buffer(int32_t index, int32_t length); + + bool IsControlQFull(); + bool IsControlQEmpty(); + bool IsDataQFull(); + bool IsDataQEmpty(); + int32_t DataQSize(); + int32_t ControlQSize(); + + int32_t get_msg_size_at(bool isData, int32_t index); + int32_t copy_msg_to_buffer(bool isData, int32_t src_index, uint8_t *dst_buffer, int32_t offset, int32_t dst_buffer_len); + int16_t control_msg_queue_add(int32_t msgtype, const char *messageName); + + std::set _RemoteSubscribers; +}; + +#endif /* _uORBFastRpcChannel_hpp_ */ diff --git a/src/prometheus_px4/src/modules/muorb/krait/CMakeLists.txt b/src/prometheus_px4/src/modules/muorb/krait/CMakeLists.txt new file mode 100644 index 0000000..aecc79f --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/krait/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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. +# +############################################################################ +include(hexagon_sdk) + +px4_add_module( + MODULE modules__muorb__krait + MAIN muorb + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + INCLUDES + ${HEXAGON_SDK_INCLUDES} + ${PX4_BINARY_DIR}/platforms/posix + SRCS + uORBKraitFastRpcChannel.cpp + px4muorb_KraitRpcWrapper.cpp + muorb_main.cpp + ) diff --git a/src/prometheus_px4/src/modules/muorb/krait/muorb_main.cpp b/src/prometheus_px4/src/modules/muorb/krait/muorb_main.cpp new file mode 100644 index 0000000..c7dec2a --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/krait/muorb_main.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +#include +#include "modules/uORB/uORBManager.hpp" +#include "uORBKraitFastRpcChannel.hpp" + +extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); } + +static void usage() +{ + warnx("Usage: muorb 'start', 'stop', 'status'"); +} + + +int +muorb_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return -EINVAL; + } + + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + if (uORB::KraitFastRpcChannel::isInstance()) { + PX4_WARN("muorb already running"); + + } else { + // register the fast rpc channel with UORB. + uORB::Manager::get_instance()->set_uorb_communicator(uORB::KraitFastRpcChannel::GetInstance()); + + // start the KaitFastRPC channel thread. + uORB::KraitFastRpcChannel::GetInstance()->Start(); + } + + return OK; + + } + + if (!strcmp(argv[1], "stop")) { + + if (uORB::KraitFastRpcChannel::isInstance()) { + uORB::KraitFastRpcChannel::GetInstance()->Stop(); + + } else { + PX4_WARN("muorb not running"); + } + + return OK; + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) { + if (uORB::KraitFastRpcChannel::isInstance()) { + PX4_WARN("muorb running"); + + } else { + PX4_WARN("muorb not running"); + } + + return OK; + } + + usage(); + return -EINVAL; +} diff --git a/src/prometheus_px4/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/prometheus_px4/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp new file mode 100644 index 0000000..a616063 --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp @@ -0,0 +1,362 @@ +/**************************************************************************** + * + * Copyright (C) 2016 Ramakrishna Kintada. 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 +#include +#include +#include "px4muorb_KraitRpcWrapper.hpp" +#include +#include +#include +#include +#include +#include + +using namespace px4muorb; + +/* Flags applied to the allocation of the shared memory for RPC */ +#define MUORB_KRAIT_FASTRPC_MEM_FLAGS 0 + +/* The ID of the HEAP to be used when allocating shared memory */ +//TODO This heap id is used for test purposes. We need to find out the correct one. +#define MUORB_KRAIT_FASTRPC_HEAP_ID 22 + +static char *_TopicNameBuffer = 0; +static const int32_t _MAX_TOPIC_NAME_BUFFER = 256; + +static uint8_t *_DataBuffer = 0; +static const uint32_t _MAX_DATA_BUFFER_SIZE = 2048; + +static bool _Initialized = false; + +// These numbers are based off the fact each fastrpc call for 64K packet is 94us. +// hence we are trying to allocation 64K of byte buffers. +static const uint32_t _MAX_TOPIC_DATA_BUFFER_SIZE = 1024; +static const uint32_t _MAX_TOPICS = 64; +static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = + _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS; +static uint8_t *_BulkTransferBuffer = 0; + +unsigned char *adsp_changed_index = 0; + +// The DSP timer can be read from this file. +#define DSP_TIMER_FILE "/sys/kernel/boot_adsp/qdsp_qtimer" + +/** + * Helper function to get timer difference between time on DSP and appsproc side. + * Usually the DSP gets started around 2s before the appsproc (Linux) side and + * therefore the clocks are not in sync. We change the clock on the DSP side but + * for this we need to find the offset first and then tell code on the DSP side. + * + * @param time_diff_us: pointer to time offset to set. + * @return: 0 on success, < 0 on error. + */ +int calc_timer_diff_to_dsp_us(int32_t *time_diff_us); + +int calc_timer_diff_to_dsp_us(int32_t *time_diff_us) +{ +#if defined(__PX4_POSIX_EXCELSIOR) + *time_diff_us = 0; + return 0; +#endif + int fd = open(DSP_TIMER_FILE, O_RDONLY); + + if (fd < 0) { + PX4_ERR("Could not open DSP timer file %s.", DSP_TIMER_FILE); + return -1; + } + + char buffer[21] {}; + int bytes_read = read(fd, buffer, sizeof(buffer)); + + if (bytes_read < 0) { + PX4_ERR("Could not read DSP timer file %s.", DSP_TIMER_FILE); + close(fd); + return -2; + } + + // Do this call right after reading to avoid latency here. + timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu + + (ts.tv_nsec / 1000); + + close(fd); + + uint64_t time_dsp; + int ret = sscanf(buffer, "%llx", &time_dsp); + + if (ret < 0) { + PX4_ERR("Could not parse DSP timer."); + return -3; + } + + // The clock count needs to get converted to us. + // The magic value of 19.2 was provided by Qualcomm. + time_dsp /= 19.2; + + // Before casting to in32_t, check if it fits. + uint64_t abs_diff = + (time_appsproc > time_dsp) ? + (time_appsproc - time_dsp) : (time_dsp - time_appsproc); + + if (abs_diff > INT32_MAX) { + PX4_ERR("Timer difference too big"); + return -4; + } + + *time_diff_us = time_appsproc - time_dsp; + + PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us", + time_dsp, time_appsproc); + PX4_DEBUG("found time_diff: %li us, %.6f s", + *time_diff_us, ((double)*time_diff_us) / 1e6); + + return 0; +} + +bool px4muorb::KraitRpcWrapper::Initialize() +{ + bool rc = true; + + PX4_DEBUG("%s Now calling rpcmem_init...", __FUNCTION__); + rpcmem_init(); + + PX4_DEBUG("%s Now calling rpcmem_alloc...", __FUNCTION__); + + _BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)); + rc = (_BulkTransferBuffer != NULL) ? true : false; + + if (!rc) { + PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers", + __FUNCTION__); + return rc; + + } else { + PX4_DEBUG( + "%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p", + __FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer); + } + + _TopicNameBuffer = (char *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_TOPIC_NAME_BUFFER * sizeof(char)); + + rc = (_TopicNameBuffer != NULL) ? true : false; + + if (!rc) { + PX4_ERR("%s rpcmem_alloc failed! for topic_name_buffer", __FUNCTION__); + rpcmem_free(_BulkTransferBuffer); + return rc; + + } else { + PX4_DEBUG("%s rpcmem_alloc passed for topic_name_buffer", __FUNCTION__); + } + + // now allocate the data buffer. + _DataBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t)); + + rc = (_DataBuffer != NULL) ? true : false; + + if (!rc) { + PX4_ERR("%s rpcmem_alloc failed! for DataBuffer", __FUNCTION__); + // free the topic name buffer; + rpcmem_free(_BulkTransferBuffer); + rpcmem_free(_TopicNameBuffer); + _TopicNameBuffer = 0; + return rc; + + } else { + PX4_DEBUG("%s rpcmem_alloc passed for data_buffer", __FUNCTION__); + } + + adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t)); + + rc = (adsp_changed_index != NULL) ? true : false; + + if (!rc) { + PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__); + + } else { + memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t)); + } + + int32_t time_diff_us; + + if (calc_timer_diff_to_dsp_us(&time_diff_us) != 0) { + rc = false; + return rc; + } + + // call muorb initialize routine. + if (px4muorb_orb_initialize() != 0) { + PX4_ERR("%s Error calling the uorb fastrpc initalize method..", + __FUNCTION__); + rc = false; + return rc; + } + + // TODO FIXME: remove this check or make it less verbose later + px4muorb_set_absolute_time_offset(time_diff_us); + + uint64_t time_dsp; + px4muorb_get_absolute_time(&time_dsp); + + uint64_t time_appsproc = hrt_absolute_time(); + + int diff = (time_dsp - time_appsproc); + + PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", + time_dsp, time_appsproc, diff); + + _Initialized = true; + return rc; +} + +bool px4muorb::KraitRpcWrapper::Terminate() +{ + if (_BulkTransferBuffer != NULL) { + rpcmem_free(_BulkTransferBuffer); + _BulkTransferBuffer = 0; + } + + if (_TopicNameBuffer != NULL) { + rpcmem_free(_TopicNameBuffer); + _TopicNameBuffer = 0; + } + + if (_DataBuffer != NULL) { + rpcmem_free(_DataBuffer); + _DataBuffer = 0; + } + + if (adsp_changed_index != NULL) { + rpcmem_free(adsp_changed_index); + adsp_changed_index = 0; + } + + _Initialized = false; + return true; +} + +int32_t px4muorb::KraitRpcWrapper::TopicAdvertised(const char *topic) +{ + return ((_Initialized) ? px4muorb_topic_advertised(topic) : -1); +} + +int32_t px4muorb::KraitRpcWrapper::TopicUnadvertised(const char *topic) +{ + return ((_Initialized) ? px4muorb_topic_unadvertised(topic) : -1); +} + +int32_t px4muorb::KraitRpcWrapper::AddSubscriber(const char *topic) +{ + return ((_Initialized) ? px4muorb_add_subscriber(topic) : -1); +} + +int32_t px4muorb::KraitRpcWrapper::RemoveSubscriber(const char *topic) +{ + return (_Initialized ? px4muorb_remove_subscriber(topic) : -1); +} + +int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic, + int32_t *status) +{ + return (_Initialized ? px4muorb_is_subscriber_present(topic, status) : -1); +} + +int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic, + int32_t length_in_bytes, const uint8_t *data) +{ + return (_Initialized ? + px4muorb_send_topic_data(topic, data, length_in_bytes) : -1); +} + +int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, + int32_t *length_in_bytes, uint8_t **data) +{ + int32_t rc = -1; + + if (_Initialized) { + rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, + _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, + length_in_bytes); + + if (rc == 0) { + *topic = _TopicNameBuffer; + *data = _DataBuffer; + + } else { + PX4_ERR("ERROR: Getting data from fastRPC link"); + } + + } else { + PX4_ERR("ERROR: FastRpcWrapper Not Initialized"); + } + + return rc; +} + +int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, + int32_t *length_in_bytes, int32_t *topic_count) +{ + int32_t rc = -1; + + if (_Initialized) { + //rc = px4muorb_receive_msg( msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes ); + rc = px4muorb_receive_bulk_data(_BulkTransferBuffer, + _MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count); + + if (rc == 0) { + *bulk_data = _BulkTransferBuffer; + + } else { + PX4_ERR("ERROR: Getting Bulk data from fastRPC link"); + } + + } else { + PX4_ERR("ERROR: FastRpcWrapper Not Initialized"); + } + + return rc; +} + +int32_t px4muorb::KraitRpcWrapper::UnblockReceiveData() +{ + return (_Initialized ? px4muorb_unblock_recieve_msg() : -1); +} diff --git a/src/prometheus_px4/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp b/src/prometheus_px4/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp new file mode 100644 index 0000000..3fec23f --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2016 Ramakrishna Kintada. 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. + * + ****************************************************************************/ +#ifndef _px4muorb_KraitRpcWrapper_hpp_ +#define _px4muorb_KraitRpcWrapper_hpp_ +#include + +namespace px4muorb +{ +class KraitRpcWrapper; +} + +class px4muorb::KraitRpcWrapper +{ +public: + /** + * Constructor + */ + KraitRpcWrapper() = default; + + /** + * destructor + */ + ~KraitRpcWrapper() = default; + + /** + * Initiatizes the rpc channel px4 muorb + */ + bool Initialize(); + + /** + * Terminate to clean up the resources. This should be called at program exit + */ + bool Terminate(); + + /** + * Muorb related functions to pub/sub of orb topic from krait to adsp + */ + int32_t TopicAdvertised(const char *topic); + int32_t TopicUnadvertised(const char *topic); + int32_t AddSubscriber(const char *topic); + int32_t RemoveSubscriber(const char *topic); + int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data); + int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data); + int32_t IsSubscriberPresent(const char *topic, int32_t *status); + int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count); + int32_t UnblockReceiveData(); +}; +#endif // _px4muorb_KraitWrapper_hpp_ diff --git a/src/prometheus_px4/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/prometheus_px4/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp new file mode 100644 index 0000000..a811db5 --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -0,0 +1,375 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 "uORBKraitFastRpcChannel.hpp" +#include +#include +#include +#include +#include +#include + +#define LOG_TAG "uORBKraitFastRpcChannel.cpp" + +uORB::KraitFastRpcChannel *uORB::KraitFastRpcChannel::_InstancePtr = nullptr; + +static void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics); + +// static initialization. +static std::string _log_file_name = "./hex_dump.txt"; + +static unsigned long _snd_msg_min = 0xFFFFFF; +static unsigned long _snd_msg_max = 0; +static double _snd_msg_avg = 0.0; +static unsigned long _snd_msg_count = 0; +static unsigned long _overall_snd_min = 0xFFFFFF; +static unsigned long _overall_snd_max = 0; +static double _overall_snd_avg = 0.0; +static unsigned long _overall_snd_count = 0; +static hrt_abstime _log_check_time = 0; +static hrt_abstime _log_check_interval = 10000000; + + +uORB::KraitFastRpcChannel::KraitFastRpcChannel() : + _RxHandler(nullptr), + _ThreadStarted(false), + _ThreadShouldExit(false) +{ + _KraitWrapper.Initialize(); +} + +int16_t uORB::KraitFastRpcChannel::topic_advertised(const char *messageName) +{ + int16_t rc = 0; + PX4_DEBUG("Before calling TopicAdvertised for [%s]\n", messageName); + rc = _KraitWrapper.TopicAdvertised(messageName); + PX4_DEBUG("Response for TopicAdvertised for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +int16_t uORB::KraitFastRpcChannel::topic_unadvertised(const char *messageName) +{ + int16_t rc = 0; + PX4_DEBUG("Before calling TopicUnadvertised for [%s]\n", messageName); + rc = _KraitWrapper.TopicUnadvertised(messageName); + PX4_DEBUG("Response for TopicUnadvertised for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + //PX4_DEBUG("Before calling AddSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.AddSubscriber(messageName); + //PX4_DEBUG("Response for AddSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +int16_t uORB::KraitFastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + //PX4_DEBUG("Before calling RemoveSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.RemoveSubscriber(messageName); + //PX4_DEBUG("Response for RemoveSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +int16_t uORB::KraitFastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + +int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + int32_t status = 0; + hrt_abstime t1, t4; + hrt_abstime t2 = 0; + hrt_abstime t3 = 0; + t1 = hrt_absolute_time(); + + if (_AdspSubscriberCache.find(std::string(messageName)) == _AdspSubscriberCache.end()) { + // check the status from adsp. as it is not cached. + if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { + _AdspSubscriberCache[messageName] = status; + _AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time(); + } + + } else { + if ((hrt_absolute_time() - _AdspSubscriberSampleTimestamp[messageName]) > _SubCacheRefreshRate) { + if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { + _AdspSubscriberCache[messageName] = status; + _AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time(); + } + } + } + + if (_AdspSubscriberCache[messageName] > 0) {// there are remote subscribers + t2 = hrt_absolute_time(); + rc = _KraitWrapper.SendData(messageName, length, data); + t3 = hrt_absolute_time(); + _snd_msg_count++; + //PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() ); + + } else { + //PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() ); + } + + t4 = hrt_absolute_time(); + _overall_snd_count++; + + if ((t4 - t1) < _overall_snd_min) { _overall_snd_min = (t4 - t1); } + + if ((t4 - t1) > _overall_snd_max) { _overall_snd_max = (t4 - t1); } + + if (_AdspSubscriberCache[messageName] > 0) { + if ((t3 - t2) < _snd_msg_min) { _snd_msg_min = (t3 - t2); } + + if ((t3 - t2) > _snd_msg_max) { _snd_msg_max = (t3 - t2); } + + _snd_msg_avg = ((double)((_snd_msg_avg * (_snd_msg_count - 1)) + + (unsigned long)(t3 - t2))) / (double)(_snd_msg_count); + } + + _overall_snd_avg = ((double)((_overall_snd_avg * (_overall_snd_count - 1)) + + (unsigned long)(t4 - t1))) / (double)(_overall_snd_count); + + if ((t4 - _log_check_time) > _log_check_interval) { + /* + PX4_DEBUG("SndMsgStats: overall_min: %lu overall_max: %lu snd_msg_min: %lu snd_msg_max: %lu", + _overall_snd_min, _overall_snd_max, + _snd_msg_min, _snd_msg_max); + PX4_DEBUG(".... overall_avg: %f (%lu) snd_msg_avg: %f (%lu)", + _overall_snd_avg, _overall_snd_count, _snd_msg_avg, _snd_msg_count); + */ + _log_check_time = t4; + _overall_snd_min = _snd_msg_min = 0xFFFFFFF; + _overall_snd_max = _snd_msg_max = 0; + _overall_snd_count = _snd_msg_count = 0; + _overall_snd_avg = _snd_msg_avg = 0.0; + } + + //PX4_DEBUG( "Response for SendMessage for [%s],len[%d] rc[%d]\n", messageName.c_str(), length, rc ); + return rc; +} + +void uORB::KraitFastRpcChannel::Start() +{ + _ThreadStarted = true; + _ThreadShouldExit = false; + pthread_attr_t recv_thread_attr; + pthread_attr_init(&recv_thread_attr); + + struct sched_param param; + (void)pthread_attr_getschedparam(&recv_thread_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 80; + (void)pthread_attr_setschedparam(&recv_thread_attr, ¶m); + + pthread_attr_setstacksize(&recv_thread_attr, 4096); + + if (pthread_create(&_RecvThread, &recv_thread_attr, thread_start, (void *)this) != 0) { + PX4_ERR("Error creating the receive thread for muorb"); + + } else { + pthread_setname_np(_RecvThread, "muorb_krait_receiver"); + } + + pthread_attr_destroy(&recv_thread_attr); +} + +void uORB::KraitFastRpcChannel::Stop() +{ + _ThreadShouldExit = true; + _KraitWrapper.UnblockReceiveData(); + //PX4_DEBUG("After calling UnblockReceiveData()...\n"); + pthread_join(_RecvThread, NULL); + //PX4_DEBUG("*** After calling pthread_join...\n"); + _ThreadStarted = false; +} + +void *uORB::KraitFastRpcChannel::thread_start(void *handler) +{ + if (handler != nullptr) { + ((uORB::KraitFastRpcChannel *)handler)->fastrpc_recv_thread(); + } + + return 0; +} + +void uORB::KraitFastRpcChannel::fastrpc_recv_thread() +{ + // sit in while loop. + int32_t rc = 0; + int32_t data_length = 0; + uint8_t *data = nullptr; + unsigned long rpc_min, rpc_max; + unsigned long orb_min, orb_max; + double rpc_avg, orb_avg; + unsigned long count = 0; + rpc_max = orb_max = 0; + rpc_min = orb_min = 0xFFFFFFFF; + rpc_avg = orb_avg = 0.0; + + int32_t num_topics = 0; + + hrt_abstime check_time = 0; + + while (!_ThreadShouldExit) { + hrt_abstime t1, t2, t3; + t1 = hrt_absolute_time(); + rc = _KraitWrapper.ReceiveBulkData(&data, &data_length, &num_topics); + + t2 = hrt_absolute_time(); + + if (rc == 0) { + //PX4_DEBUG( "Num of topics Received: %d", num_topics ); + int32_t bytes_processed = 0; + + for (int i = 0; i < num_topics; ++i) { + uint8_t *new_pkt = &(data[bytes_processed]); + struct BulkTransferHeader *header = (struct BulkTransferHeader *)new_pkt; + char *messageName = (char *)(new_pkt + sizeof(struct BulkTransferHeader)); + uint16_t check_msg_len = strlen(messageName); + + if (header->_MsgNameLen != (check_msg_len + 1)) { + PX4_ERR("Error: Packing error. Sent Msg Len. of[%d] but strlen returned:[%d]", header->_MsgNameLen, check_msg_len); + PX4_ERR("Error: NumTopics: %d processing topic: %d msgLen[%d] dataLen[%d] data_len[%d] bytes processed: %d", + num_topics, i, header->_MsgNameLen, header->_DataLen, data_length, bytes_processed); + DumpData(data, data_length, num_topics); + break; + } + + uint8_t *topic_data = (uint8_t *)(messageName + strlen(messageName) + 1); + + if (_RxHandler != nullptr) { + if (header->_MsgType == _DATA_MSG_TYPE) { + //PX4_DEBUG( "Received topic data for: [%s] len[%d]\n", messageName, data_length ); + _RxHandler->process_received_message(messageName, + header->_DataLen, topic_data); + + } else if (header->_MsgType == _CONTROL_MSG_TYPE_ADVERTISE) { + PX4_DEBUG("Received topic advertise message for: [%s] len[%d]\n", messageName, data_length); + _RxHandler->process_remote_topic(messageName, true); + + } else if (header->_MsgType == _CONTROL_MSG_TYPE_UNADVERTISE) { + PX4_DEBUG("Received topic unadvertise message for: [%s] len[%d]\n", messageName, data_length); + _RxHandler->process_remote_topic(messageName, false); + } + } + + bytes_processed += header->_MsgNameLen + header->_DataLen + sizeof(struct BulkTransferHeader); + } + + } else { + PX4_DEBUG("Error: Getting data over fastRPC channel\n"); + break; + } + + t3 = hrt_absolute_time(); + count++; + + if ((unsigned long)(t2 - t1) < rpc_min) { + rpc_min = (unsigned long)(t2 - t1); + } + + if ((unsigned long)(t2 - t1) > rpc_max) { + rpc_max = (unsigned long)(t2 - t1); + } + + if ((unsigned long)(t3 - t2) < orb_min) { + orb_min = (unsigned long)(t3 - t2); + } + + if ((unsigned long)(t3 - t2) > orb_max) { + orb_max = (unsigned long)(t3 - t2); + } + + rpc_avg = ((double)((rpc_avg * (count - 1)) + (unsigned long)(t2 - t1))) / (double)(count); + orb_avg = ((double)((orb_avg * (count - 1)) + (unsigned long)(t3 - t2))) / (double)(count); + + if ((unsigned long)(t3 - check_time) >= 10000000) { + //PX4_DEBUG("Krait RPC Stats : rpc_min: %lu rpc_max: %lu rpc_avg: %f", rpc_min, rpc_max, rpc_avg); + //PX4_DEBUG("Krait RPC(orb) Stats: orb_min: %lu orb_max: %lu orb_avg: %f", orb_min, orb_max, orb_avg); + check_time = t3; + rpc_max = orb_max = 0; + rpc_min = orb_min = 0xFFFFFF; + orb_avg = 0; + rpc_avg = 0; + count = 0; + } + + //PX4_DEBUG("MsgName: %30s, t1: %lu, t2: %lu, t3: %lu, dt1: %lu, dt2: %lu",name, (unsigned long) t1, (unsigned long) t2, (unsigned long) t3, + // (unsigned long) (t2-t1), (unsigned long) (t3-t2)); + } + + PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n"); +} + +void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics) +{ + FILE *fp = fopen(_log_file_name.c_str(), "a+"); + + if (fp == nullptr) { + PX4_ERR("Error unable to open log file[%s]", _log_file_name.c_str()); + return; + } + + fprintf(fp, "===== Data Len[%d] num_topics[%d] ======\n", length, num_topics); + + for (int i = 0; i < length; i += 16) { + int remaining_chars = length - i; + remaining_chars = (remaining_chars >= 16) ? 16 : remaining_chars; + + fprintf(fp, "%p - ", &(buffer[i])); + + for (int j = 0; j < remaining_chars; j++) { + fprintf(fp, " %02X", buffer[i + j]); + + if (j == 7) { + fprintf(fp, " -"); + } + } + + fprintf(fp, " "); + + for (int j = 0; j < remaining_chars; j++) { + fprintf(fp, "%c", (char)buffer[i + j ]); + } + + fprintf(fp, "\n"); + } + + fclose(fp); +} + diff --git a/src/prometheus_px4/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/prometheus_px4/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp new file mode 100644 index 0000000..25fad8e --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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. + * + ****************************************************************************/ + +#ifndef _uORBKraitFastRpcChannel_hpp_ +#define _uORBKraitFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include "px4muorb_KraitRpcWrapper.hpp" +#include +#include "drivers/drv_hrt.h" + +namespace uORB +{ +class KraitFastRpcChannel; +} + +class uORB::KraitFastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::KraitFastRpcChannel *GetInstance() + { + if (_InstancePtr == nullptr) { + _InstancePtr = new uORB::KraitFastRpcChannel(); + } + + return _InstancePtr; + } + + /** + * Static method to check if there is an instance. + */ + static bool isInstance() + { + return (_InstancePtr != nullptr); + } + + /** + * @brief Interface to notify the remote entity of a topic being advertised. + * + * @param messageName + * This represents the uORB message name(aka topic); This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t topic_advertised(const char *messageName); + + /** + * @brief Interface to notify the remote entity of a topic being unadvertised + * and is no longer publishing messages. + * + * @param messageName + * This represents the uORB message name(aka topic); This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t topic_unadvertised(const char *messageName); + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + + void Start(); + void Stop(); + +private: // data members + static uORB::KraitFastRpcChannel *_InstancePtr; + uORBCommunicator::IChannelRxHandler *_RxHandler; + pthread_t _RecvThread; + bool _ThreadStarted; + bool _ThreadShouldExit; + + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + static const int32_t _CONTROL_MSG_TYPE_ADVERTISE = 4; + static const int32_t _CONTROL_MSG_TYPE_UNADVERTISE = 5; + + struct BulkTransferHeader { + uint16_t _MsgType; + uint16_t _MsgNameLen; + uint16_t _DataLen; + }; + + px4muorb::KraitRpcWrapper _KraitWrapper; + + std::map _AdspSubscriberCache; + std::map _AdspSubscriberSampleTimestamp; + //hrt_abstime _SubCacheSampleTimestamp; + static const hrt_abstime _SubCacheRefreshRate = 1000000; // 1 second; + +private://class members. + /// constructor. + KraitFastRpcChannel(); + + static void *thread_start(void *handler); + + void fastrpc_recv_thread(); + +}; + +#endif /* _uORBKraitFastRpcChannel_hpp_ */ diff --git a/src/prometheus_px4/src/modules/muorb/test/CMakeLists.txt b/src/prometheus_px4/src/modules/muorb/test/CMakeLists.txt new file mode 100644 index 0000000..341f2d9 --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/test/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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__muorb__test__muorb_test + MAIN muorb_test + SRCS + muorb_test_main.cpp + muorb_test_start_posix.cpp + muorb_test_example.cpp + DEPENDS + ) diff --git a/src/prometheus_px4/src/modules/muorb/test/muorb_test_example.cpp b/src/prometheus_px4/src/modules/muorb/test/muorb_test_example.cpp new file mode 100644 index 0000000..8cb9804 --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/test/muorb_test_example.cpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 hello_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include "muorb_test_example.h" +#include +#include +#include +#include +#include +#include + +px4::AppState MuorbTestExample::appState; + +int MuorbTestExample::main() +{ + int rc; + appState.setRunning(true); + rc = PingPongTest(); + appState.setRunning(false); + return rc; +} + +int MuorbTestExample::DefaultTest() +{ + int i = 0; + + uORB::Subscription sub_vc{ORB_ID(vehicle_command)}; + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; + uORB::Publication pub_id{ORB_ID(esc_status)}; + pub_id.publish(m_esc_status); + + while (!appState.exitRequested() && i < 100) { + + PX4_DEBUG("[%d] Doing work...", i); + + if (!pub_id.publish(m_esc_status)) { + PX4_ERR("[%d]Error publishing the esc status message for iter", i); + break; + } + + if (sub_vc.updated()) { + PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); + + if (!sub_vc.copy(&m_vc)) { + PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); + break; + } + + if (!vcmd_pub.publish(m_vc)) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; + } + + } else { + PX4_ERR("[%d]Error checking the updated status for vechile command... ", i); + PX4_DEBUG("[%d] VC topic is not updated", i); + break; + } + + ++i; + } + + return 0; +} + +int MuorbTestExample::PingPongTest() +{ + int i = 0; + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; + uORB::Subscription sub_esc_status{ORB_ID(esc_status)}; + + while (!appState.exitRequested()) { + + PX4_INFO("[%d] Doing work...", i); + + if (sub_esc_status.updated()) { + PX4_INFO("[%d]ESC status is updated... reading new value", i); + + if (!sub_esc_status.copy(&m_esc_status)) { + PX4_ERR("[%d]Error calling orb copy for esc status... ", i); + break; + } + + if (!vcmd_pub.publish(m_vc)) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; + } + + } else { + PX4_INFO("[%d] esc status topic is not updated", i); + } + + // sleep for 1 sec. + usleep(1000000); + + ++i; + } + + return 0; +} diff --git a/src/prometheus_px4/src/modules/muorb/test/muorb_test_example.h b/src/prometheus_px4/src/modules/muorb/test/muorb_test_example.h new file mode 100644 index 0000000..1925ba1 --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/test/muorb_test_example.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 hello_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include +#include "uORB/topics/esc_status.h" +#include "uORB/topics/vehicle_command.h" + +class MuorbTestExample +{ +public: + MuorbTestExample() {} + + ~MuorbTestExample() {} + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +private: + int DefaultTest(); + int PingPongTest(); + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc = {}; + +}; diff --git a/src/prometheus_px4/src/modules/muorb/test/muorb_test_main.cpp b/src/prometheus_px4/src/modules/muorb/test/muorb_test_main.cpp new file mode 100644 index 0000000..551b18e --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/test/muorb_test_main.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 hello_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include +#include "muorb_test_example.h" +#include +#include "uORB/uORBManager.hpp" +#include "../../../modules/muorb/krait/uORBKraitFastRpcChannel.hpp" + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "muorb_test"); + + PX4_DEBUG("muorb_test"); + + MuorbTestExample hello; + hello.main(); + + PX4_DEBUG("goodbye"); + return 0; +} diff --git a/src/prometheus_px4/src/modules/muorb/test/muorb_test_start_posix.cpp b/src/prometheus_px4/src/modules/muorb/test/muorb_test_start_posix.cpp new file mode 100644 index 0000000..3ca3b2c --- /dev/null +++ b/src/prometheus_px4/src/modules/muorb/test/muorb_test_start_posix.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 hello_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "muorb_test_example.h" +#include +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); + +static void usage() +{ + PX4_DEBUG("usage: muorb_test {start|stop|status}"); +} +int muorb_test_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("already running"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("muorb_test", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char *const *)argv); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + MuorbTestExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("is running"); + + } else { + PX4_DEBUG("not started"); + } + + return 0; + } + + usage(); + return 1; +} diff --git a/src/prometheus_px4/src/modules/navigator/CMakeLists.txt b/src/prometheus_px4/src/modules/navigator/CMakeLists.txt new file mode 100644 index 0000000..4a237cc --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/CMakeLists.txt @@ -0,0 +1,62 @@ +############################################################################ +# +# 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(GeofenceBreachAvoidance) + +px4_add_module( + MODULE modules__navigator + MAIN navigator + SRCS + navigator_main.cpp + navigator_mode.cpp + mission_block.cpp + mission.cpp + loiter.cpp + rtl.cpp + takeoff.cpp + land.cpp + precland.cpp + mission_feasibility_checker.cpp + geofence.cpp + enginefailure.cpp + gpsfailure.cpp + follow_target.cpp + DEPENDS + git_ecl + ecl_geo + landing_slope + geofence_breach_avoidance + motion_planning + ) + +px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman) \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt new file mode 100644 index 0000000..0696dbc --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(geofence_breach_avoidance + geofence_breach_avoidance.cpp + geofence_breach_avoidance.h +) + +px4_add_functional_gtest(SRC GeofenceBreachAvoidanceTest.cpp LINKLIBS geofence_breach_avoidance modules__navigator ecl_geo motion_planning) diff --git a/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp new file mode 100644 index 0000000..d43bd52 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp @@ -0,0 +1,305 @@ +/**************************************************************************** + * + * 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 +#include "geofence_breach_avoidance.h" +#include "fake_geofence.hpp" +#include "dataman_mocks.hpp" +#include + +using namespace matrix; + +class GeofenceBreachAvoidanceTest : public ::testing::Test +{ +public: + void SetUp() override + { + param_control_autosave(false); + } +}; + +TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance) +{ + + GeofenceBreachAvoidance gf_avoidance(nullptr); + struct map_projection_reference_s ref = {}; + Vector2d home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + + Vector2f waypoint_north_east_local(1.0, 1.0); + waypoint_north_east_local = waypoint_north_east_local.normalized() * 10.5; + Vector2d waypoint_north_east_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 10.5); + + float x, y; + map_projection_project(&ref, waypoint_north_east_global(0), waypoint_north_east_global(1), &x, &y); + Vector2f waypoint_north_east_reprojected(x, y); + + EXPECT_FLOAT_EQ(waypoint_north_east_local(0), waypoint_north_east_reprojected(0)); + EXPECT_FLOAT_EQ(waypoint_north_east_local(1), waypoint_north_east_reprojected(1)); + + + Vector2f waypoint_south_west_local = -waypoint_north_east_local; + + Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5); + + map_projection_project(&ref, waypoint_southwest_global(0), waypoint_southwest_global(1), &x, &y); + Vector2f waypoint_south_west_reprojected(x, y); + + EXPECT_FLOAT_EQ(waypoint_south_west_local(0), waypoint_south_west_reprojected(0)); + EXPECT_FLOAT_EQ(waypoint_south_west_local(1), waypoint_south_west_reprojected(1)); + + + Vector2d same_as_home_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 0.0); + + EXPECT_LT(Vector2d(home_global - same_as_home_global).norm(), 1e-4); +} + +TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing) +{ + GeofenceBreachAvoidance gf_avoidance(nullptr); + FakeGeofence geo; + struct map_projection_reference_s ref = {}; + Vector2d home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + + geofence_violation_type_u gf_violation; + gf_violation.flags.fence_violation = true; + + gf_avoidance.setHorizontalTestPointDistance(20.0f); + gf_avoidance.setTestPointBearing(0.0f); + gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0); + + + Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); + + // the expected loiter point is located test_point_distance behind + Vector2d loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, -20.0f); + + EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0)); + EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1)); + + + geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::LEFT_INSIDE_RIGHT_OUTSIDE); + loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); + + loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, -M_PI_F * 0.5f, 20.0f); + + EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0)); + EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1)); + + geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::RIGHT_INSIDE_LEFT_OUTSIDE); + loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); + + loiter_point_lat_lon_expected = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.5f, 20.0f); + + EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), loiter_point_lat_lon_expected(0)); + EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), loiter_point_lat_lon_expected(1)); + + gf_violation.flags.fence_violation = false; + loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); + + EXPECT_FLOAT_EQ(loiter_point_lat_lon(0), home_global(0)); + EXPECT_FLOAT_EQ(loiter_point_lat_lon(1), home_global(1)); +} + +TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor) +{ + GeofenceBreachAvoidance gf_avoidance(nullptr); + FakeGeofence geo; + struct map_projection_reference_s ref = {}; + Vector2d home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + + param_t param = param_handle(px4::params::MPC_ACC_HOR); + + float value = 3; + param_set(param, &value); + + param = param_handle(px4::params::MPC_ACC_HOR_MAX); + value = 5; + param_set(param, &value); + + param = param_handle(px4::params::MPC_JERK_AUTO); + value = 8; + param_set(param, &value); + + geofence_violation_type_u gf_violation; + gf_violation.flags.fence_violation = true; + + gf_avoidance.setHorizontalTestPointDistance(30.0f); + gf_avoidance.setTestPointBearing(0.0f); + gf_avoidance.setCurrentPosition(home_global(0), home_global(1), 0); + // vehicle is approaching the fence at a crazy velocity + gf_avoidance.setHorizontalVelocity(1000.0f); + gf_avoidance.computeBrakingDistanceMultirotor(); + + geo.setProbeFunctionBehavior(FakeGeofence::ProbeFunction::GF_BOUNDARY_20M_AHEAD); + + Vector2d loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); + + Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, + 20.0f - gf_avoidance.getMinHorDistToFenceMulticopter()); + + float error = get_distance_to_next_waypoint(loiter_point(0), loiter_point(1), loiter_point_predicted(0), + loiter_point_predicted(1)); + + EXPECT_LE(error, 0.5f); + + // vehicle is approaching fenc slowly, plenty of time to brake + gf_avoidance.setHorizontalVelocity(0.1f); + gf_avoidance.computeBrakingDistanceMultirotor(); + + loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); + loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, 0.0f, + gf_avoidance.computeBrakingDistanceMultirotor()); + + + error = get_distance_to_next_waypoint(loiter_point(0), loiter_point(1), loiter_point_predicted(0), + loiter_point_predicted(1)); + + EXPECT_LE(error, 0.0f); + + gf_violation.flags.fence_violation = false; + loiter_point = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); + + EXPECT_LT(Vector2d(loiter_point - home_global).norm(), 1e-4); +} + +TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing) +{ + GeofenceBreachAvoidance gf_avoidance(nullptr); + const float current_alt_amsl = 100.0f; + const float vertical_test_point_dist = 10.0f; + + gf_avoidance.setVerticalTestPointDistance(vertical_test_point_dist); + gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); // just care about altitude + geofence_violation_type_u gf_violation; + gf_violation.flags.max_altitude_exceeded = true; + + float loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation); + + EXPECT_EQ(loiter_alt, current_alt_amsl - 2 * vertical_test_point_dist); + + gf_violation.flags.max_altitude_exceeded = false; + + loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation); + + EXPECT_EQ(loiter_alt, current_alt_amsl); +} + +TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter) +{ + GeofenceBreachAvoidance gf_avoidance(nullptr); + const float climbrate = 10.0f; + const float current_alt_amsl = 100.0f; + geofence_violation_type_u gf_violation; + gf_violation.flags.max_altitude_exceeded = true; + + gf_avoidance.setClimbRate(climbrate); + gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); + gf_avoidance.computeVerticalBrakingDistanceMultirotor(); + + float loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(gf_violation); + + EXPECT_EQ(loiter_alt_amsl, current_alt_amsl + gf_avoidance.computeVerticalBrakingDistanceMultirotor() - + gf_avoidance.getMinVertDistToFenceMultirotor()); + + gf_violation.flags.max_altitude_exceeded = false; + + loiter_alt_amsl = gf_avoidance.generateLoiterAltitudeForMulticopter(gf_violation); + + EXPECT_EQ(loiter_alt_amsl, current_alt_amsl); +} + +TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter) +{ + GeofenceBreachAvoidance gf_avoidance(nullptr); + FakeGeofence geo; + struct map_projection_reference_s ref = {}; + Vector2d home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + geofence_violation_type_u gf_violation; + gf_violation.flags.dist_to_home_exceeded = true; + + const float hor_vel = 8.0f; + const float test_point_distance = 30.0f; + const float test_point_bearing = 0.0f; + const float max_dist_to_home = 100.0f; + + gf_avoidance.setHorizontalVelocity(hor_vel); + gf_avoidance.computeBrakingDistanceMultirotor(); + gf_avoidance.setHorizontalTestPointDistance(test_point_distance); + gf_avoidance.setTestPointBearing(test_point_bearing); + + Vector2d current_pos = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, 90.0f); + gf_avoidance.setCurrentPosition(current_pos(0), current_pos(1), 0); + gf_avoidance.setMaxHorDistHome(max_dist_to_home); + gf_avoidance.setHomePosition(home_global(0), home_global(1), 0); + + Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForMultirotor(gf_violation, &geo); + + Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, + max_dist_to_home - gf_avoidance.getMinHorDistToFenceMulticopter()); + + EXPECT_LT(Vector2d(loiter_point_predicted - loiter_point_lat_lon).norm(), 1e-4); +} + +TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing) +{ + GeofenceBreachAvoidance gf_avoidance(nullptr); + FakeGeofence geo; + struct map_projection_reference_s ref = {}; + Vector2d home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + geofence_violation_type_u gf_violation; + gf_violation.flags.dist_to_home_exceeded = true; + + const float test_point_distance = 30.0f; + const float max_dist_to_home = 100.0f; + const float test_point_bearing = 0.0f; + + gf_avoidance.setHorizontalTestPointDistance(test_point_distance); + gf_avoidance.setTestPointBearing(test_point_bearing); + + Vector2d current_pos = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, 90.0f); + gf_avoidance.setCurrentPosition(current_pos(0), current_pos(1), 0); + gf_avoidance.setMaxHorDistHome(max_dist_to_home); + gf_avoidance.setHomePosition(home_global(0), home_global(1), 0); + + Vector2d loiter_point_lat_lon = gf_avoidance.generateLoiterPointForFixedWing(gf_violation, &geo); + + Vector2d loiter_point_predicted = gf_avoidance.waypointFromBearingAndDistance(home_global, test_point_bearing, + max_dist_to_home - 2.0f * test_point_distance); + + EXPECT_LT(Vector2d(loiter_point_predicted - loiter_point_lat_lon).norm(), 1e-4); +} diff --git a/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp new file mode 100644 index 0000000..e6605b5 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 dataman_mocks.h + * Provides a minimal dataman implementation to compile against for testing + * + * @author Roman Bapst + * @author Julian Kent + */ +#pragma once + +#include +extern "C" { + __EXPORT ssize_t + dm_read( + dm_item_t item, /* The item type to retrieve */ + unsigned index, /* The index of the item */ + void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ + ) {return 0;}; + + /** write to the data manager store */ + __EXPORT ssize_t + dm_write( + dm_item_t item, /* The item type to store */ + unsigned index, /* The index of the item */ + dm_persitence_t persistence, /* The persistence level of this item */ + const void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ + ) {return 0;}; + + /** + * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated + * atomically). + * Note that this lock is independent from dm_read & dm_write calls. + * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) + */ + __EXPORT int + dm_lock( + dm_item_t item /* The item type to lock */ + ) {return 0;}; + + /** + * Try to lock all items of a type (@see sem_trywait()). + * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) + */ + __EXPORT int + dm_trylock( + dm_item_t item /* The item type to lock */ + ) {return 0;}; + + /** Unlock all items of a type */ + __EXPORT void + dm_unlock( + dm_item_t item /* The item type to unlock */ + ) {}; + + /** Erase all items of this type */ + __EXPORT int + dm_clear( + dm_item_t item /* The item type to clear */ + ) {return 0;}; + + /** Tell the data manager about the type of the last reset */ + __EXPORT int + dm_restart( + dm_reset_reason restart_type /* The last reset type */ + ) {return 0;}; +} + diff --git a/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp new file mode 100644 index 0000000..3a1ff00 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * 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 fake_geofence.h + * Provides a fake geofence interface to use for testing + * + * @author Roman Bapst + * @author Julian Kent + */ +#pragma once + +#include"../geofence.h" +#include + +class FakeGeofence : public Geofence +{ +public: + FakeGeofence() : + Geofence(nullptr) + {}; + + virtual ~FakeGeofence() {}; + + bool isInsidePolygonOrCircle(double lat, double lon, float altitude) override + { + switch (_probe_function_behavior) { + case ProbeFunction::ALL_POINTS_OUTSIDE: { + return _allPointsOutside(lat, lon, altitude); + } + + case ProbeFunction::LEFT_INSIDE_RIGHT_OUTSIDE: { + return _left_inside_right_outside(lat, lon, altitude); + } + + case ProbeFunction::RIGHT_INSIDE_LEFT_OUTSIDE: { + return _right_inside_left_outside(lat, lon, altitude); + } + + case ProbeFunction::GF_BOUNDARY_20M_AHEAD: { + return _gf_boundary_is_20m_north(lat, lon, altitude); + } + + default: + return _allPointsOutside(lat, lon, altitude); + } + } + + enum class ProbeFunction { + ALL_POINTS_OUTSIDE = 0, + LEFT_INSIDE_RIGHT_OUTSIDE, + RIGHT_INSIDE_LEFT_OUTSIDE, + GF_BOUNDARY_20M_AHEAD + }; + + void setProbeFunctionBehavior(ProbeFunction func) {_probe_function_behavior = func;} + + +private: + + ProbeFunction _probe_function_behavior = ProbeFunction::ALL_POINTS_OUTSIDE; + + bool _flag_on_left = true; + bool _flag_on_right = false; + + bool _allPointsOutside(double lat, double lon, float alt) + { + return false; + } + + bool _left_inside_right_outside(double lat, double lon, float alt) + { + if (_flag_on_left) { + _flag_on_left = false; + return true; + + } else { + return false; + } + } + + bool _right_inside_left_outside(double lat, double lon, float alt) + { + if (_flag_on_right) { + _flag_on_right = false; + return true; + + } else { + _flag_on_right = true; + return false; + } + } + + bool _gf_boundary_is_20m_north(double lat, double lon, float alt) + { + struct map_projection_reference_s ref = {}; + matrix::Vector2 home_global(42.1, 8.2); + map_projection_init(&ref, home_global(0), home_global(1)); + + float x, y; + map_projection_project(&ref, lat, lon, &x, &y); + matrix::Vector2f waypoint_local(x, y); + + if (waypoint_local(0) >= 20.0f) { + return false; + } + + return true; + } +}; diff --git a/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp new file mode 100644 index 0000000..c965e6f --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * 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 "geofence_breach_avoidance.h" +#include +#include + +using Vector2d = matrix::Vector2; + + +GeofenceBreachAvoidance::GeofenceBreachAvoidance(ModuleParams *parent) : + ModuleParams(parent) +{ + _paramHandle.param_mpc_jerk_max = param_find("MPC_JERK_MAX"); + _paramHandle.param_mpc_acc_hor = param_find("MPC_ACC_HOR"); + _paramHandle.param_mpc_acc_hor_max = param_find("MPC_ACC_HOR_MAX"); + _paramHandle.param_mpc_jerk_auto = param_find("MPC_JERK_AUTO"); + _paramHandle.param_mpc_acc_up_max = param_find("MPC_ACC_UP_MAX"); + _paramHandle.param_mpc_acc_down_max = param_find("MPC_ACC_DOWN_MAX"); + + updateParameters(); +} + +void GeofenceBreachAvoidance::updateParameters() +{ + ModuleParams::updateParams(); + param_get(_paramHandle.param_mpc_jerk_max, &_params.param_mpc_jerk_max); + param_get(_paramHandle.param_mpc_acc_hor, &_params.param_mpc_acc_hor); + param_get(_paramHandle.param_mpc_acc_hor_max, &_params.param_mpc_acc_hor_max); + param_get(_paramHandle.param_mpc_jerk_auto, &_params.param_mpc_jerk_auto); + param_get(_paramHandle.param_mpc_acc_up_max, &_params.param_mpc_acc_up_max); + param_get(_paramHandle.param_mpc_acc_down_max, &_params.param_mpc_acc_down_max); + + updateMinHorDistToFenceMultirotor(); +} + +void GeofenceBreachAvoidance::setCurrentPosition(double lat, double lon, float alt) +{ + _current_pos_lat_lon = Vector2d(lat, lon); + _current_alt_amsl = alt; +} + +void GeofenceBreachAvoidance::setHomePosition(double lat, double lon, float alt) +{ + _home_lat_lon(0) = lat; + _home_lat_lon(1) = lon; + _home_alt_amsl = alt; +} + +matrix::Vector2 GeofenceBreachAvoidance::waypointFromBearingAndDistance(matrix::Vector2 + current_pos_lat_lon, float test_point_bearing, float test_point_distance) +{ + // TODO: Remove this once the underlying geo function has been fixed + if (test_point_distance < 0.0f) { + test_point_distance *= -1.0f; + test_point_bearing = matrix::wrap_2pi(test_point_bearing + M_PI_F); + } + + double fence_violation_test_point_lat, fence_violation_test_point_lon; + waypoint_from_heading_and_distance(current_pos_lat_lon(0), current_pos_lat_lon(1), test_point_bearing, + test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon); + + return Vector2d(fence_violation_test_point_lat, fence_violation_test_point_lon); +} + +Vector2d +GeofenceBreachAvoidance::getFenceViolationTestPoint() +{ + return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _test_point_distance); +} + +Vector2d +GeofenceBreachAvoidance::generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence) +{ + if (violation_type.flags.fence_violation) { + const float bearing_90_left = matrix::wrap_2pi(_test_point_bearing - M_PI_F * 0.5f); + const float bearing_90_right = matrix::wrap_2pi(_test_point_bearing + M_PI_F * 0.5f); + + double loiter_center_lat, loiter_center_lon; + double fence_violation_test_point_lat, fence_violation_test_point_lon; + + waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_90_left, + _test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon); + + const bool left_side_is_inside_fence = geofence->isInsidePolygonOrCircle(fence_violation_test_point_lat, + fence_violation_test_point_lon, _current_alt_amsl); + + waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_90_right, + _test_point_distance, &fence_violation_test_point_lat, &fence_violation_test_point_lon); + + const bool right_side_is_inside_fence = geofence->isInsidePolygonOrCircle(fence_violation_test_point_lat, + fence_violation_test_point_lon, _current_alt_amsl); + + float bearing_to_loiter_point; + + if (right_side_is_inside_fence && !left_side_is_inside_fence) { + bearing_to_loiter_point = bearing_90_right; + + } else if (left_side_is_inside_fence && !right_side_is_inside_fence) { + bearing_to_loiter_point = bearing_90_left; + + } else { + bearing_to_loiter_point = matrix::wrap_2pi(_test_point_bearing + M_PI_F); + } + + waypoint_from_heading_and_distance(_current_pos_lat_lon(0), _current_pos_lat_lon(1), bearing_to_loiter_point, + _test_point_distance, &loiter_center_lat, &loiter_center_lon); + + return Vector2d(loiter_center_lat, loiter_center_lon); + + } else if (violation_type.flags.dist_to_home_exceeded) { + + return waypointFromHomeToTestPointAtDist(math::max(_max_hor_dist_home - 2 * _test_point_distance, 0.0f)); + + } else { + return _current_pos_lat_lon; + } +} + +Vector2d +GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_type_u violation_type, Geofence *geofence) +{ + + if (violation_type.flags.fence_violation) { + float current_distance = _test_point_distance * 0.5f; + float current_min = 0.0f; + float current_max = _test_point_distance; + Vector2d test_point; + + // binary search for the distance from the drone to the geofence in the given direction + while (abs(current_max - current_min) > 0.5f) { + test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance); + + if (!geofence->isInsidePolygonOrCircle(test_point(0), test_point(1), _current_alt_amsl)) { + current_max = current_distance; + + } else { + current_min = current_distance; + } + + current_distance = (current_max + current_min) * 0.5f; + } + + test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance); + + if (_multirotor_braking_distance > current_distance - _min_hor_dist_to_fence_mc) { + return waypointFromBearingAndDistance(test_point, _test_point_bearing + M_PI_F, _min_hor_dist_to_fence_mc); + + } else { + return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance); + } + + } else if (violation_type.flags.dist_to_home_exceeded) { + + return waypointFromHomeToTestPointAtDist(math::max(_max_hor_dist_home - _min_hor_dist_to_fence_mc, 0.0f)); + + } else { + if (_velocity_hor_abs > 0.5f) { + return waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, _multirotor_braking_distance); + + } else { + return _current_pos_lat_lon; + } + } +} + +float GeofenceBreachAvoidance::generateLoiterAltitudeForFixedWing(geofence_violation_type_u violation_type) +{ + if (violation_type.flags.max_altitude_exceeded) { + return _current_alt_amsl - 2.0f * _vertical_test_point_distance; + + } else { + return _current_alt_amsl; + } +} + +float GeofenceBreachAvoidance::generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type) +{ + if (violation_type.flags.max_altitude_exceeded) { + return _current_alt_amsl + _multirotor_vertical_braking_distance - _min_vert_dist_to_fence_mc; + + } else { + return _current_alt_amsl; + } +} + +float GeofenceBreachAvoidance::computeBrakingDistanceMultirotor() +{ + const float accel_delay_max = math::max(_params.param_mpc_acc_hor, _params.param_mpc_acc_hor_max); + VelocitySmoothing predictor(accel_delay_max, _velocity_hor_abs, 0.f); + predictor.setMaxVel(_velocity_hor_abs); + predictor.setMaxAccel(_params.param_mpc_acc_hor); + predictor.setMaxJerk(_params.param_mpc_jerk_auto); + predictor.updateDurations(0.f); + predictor.updateTraj(predictor.getTotalTime()); + + _multirotor_braking_distance = predictor.getCurrentPosition() + (GEOFENCE_CHECK_INTERVAL_US / 1e6f) * + _velocity_hor_abs; + + return _multirotor_braking_distance; +} + +float GeofenceBreachAvoidance::computeVerticalBrakingDistanceMultirotor() +{ + const float accel_delay_max = math::max(_params.param_mpc_acc_up_max, _params.param_mpc_acc_down_max); + const float vertical_vel_abs = fabsf(_climbrate); + + + VelocitySmoothing predictor(accel_delay_max, vertical_vel_abs, 0.f); + predictor.setMaxVel(vertical_vel_abs); + predictor.setMaxAccel(_climbrate > 0 ? _params.param_mpc_acc_down_max : _params.param_mpc_acc_up_max); + predictor.setMaxJerk(_params.param_mpc_jerk_auto); + predictor.updateDurations(0.f); + predictor.updateTraj(predictor.getTotalTime()); + + _multirotor_vertical_braking_distance = predictor.getCurrentPosition() + (GEOFENCE_CHECK_INTERVAL_US / 1e6f) * + vertical_vel_abs; + + _multirotor_vertical_braking_distance = matrix::sign(_climbrate) * _multirotor_vertical_braking_distance; + + return _multirotor_vertical_braking_distance; +} + +void GeofenceBreachAvoidance::updateMinHorDistToFenceMultirotor() +{ + const float accel_delay_max = math::max(_params.param_mpc_acc_hor, _params.param_mpc_acc_hor_max); + VelocitySmoothing predictor(accel_delay_max, 0.0f, 0.f); + predictor.setMaxVel(0.0f); + predictor.setMaxAccel(_params.param_mpc_acc_hor); + predictor.setMaxJerk(_params.param_mpc_jerk_auto); + predictor.updateDurations(0.f); + predictor.updateTraj(predictor.getTotalTime()); + + _min_hor_dist_to_fence_mc = 2.0f * predictor.getCurrentPosition(); + +} + +void GeofenceBreachAvoidance::updateMinVertDistToFenceMultirotor() +{ + const float accel_delay_max = math::max(_params.param_mpc_acc_up_max, _params.param_mpc_acc_down_max); + + VelocitySmoothing predictor(accel_delay_max, 0, 0.f); + predictor.setMaxVel(0); + predictor.setMaxAccel(_params.param_mpc_acc_down_max); + predictor.setMaxJerk(_params.param_mpc_jerk_auto); + predictor.updateDurations(0.f); + predictor.updateTraj(predictor.getTotalTime()); + + _min_vert_dist_to_fence_mc = 2.0f * predictor.getCurrentPosition(); +} + +Vector2d GeofenceBreachAvoidance::waypointFromHomeToTestPointAtDist(float distance) +{ + Vector2d test_point = getFenceViolationTestPoint(); + float bearing_home_current_pos = get_bearing_to_next_waypoint(_home_lat_lon(0), _home_lat_lon(1), test_point(0), + test_point(1)); + double loiter_center_lat, loiter_center_lon; + + waypoint_from_heading_and_distance(_home_lat_lon(0), _home_lat_lon(1), bearing_home_current_pos, distance, + &loiter_center_lat, &loiter_center_lon); + + return Vector2d(loiter_center_lat, loiter_center_lon); +} diff --git a/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h new file mode 100644 index 0000000..1a6c63e --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.h @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * 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 +#include +#include "../geofence.h" +#include + + +class Geofence; + +#define GEOFENCE_CHECK_INTERVAL_US 200000 + +union geofence_violation_type_u { + struct { + bool dist_to_home_exceeded: 1; ///< 0 - distance to home exceeded + bool max_altitude_exceeded: 1; ///< 1 - maximum altitude exceeded + bool fence_violation: 1; ///< 2- violation of user defined fence + } flags; + uint8_t value; +}; + +class GeofenceBreachAvoidance : public ModuleParams +{ +public: + GeofenceBreachAvoidance(ModuleParams *parent); + + ~GeofenceBreachAvoidance() = default; + + matrix::Vector2 getFenceViolationTestPoint(); + + matrix::Vector2 waypointFromBearingAndDistance(matrix::Vector2 current_pos_lat_lon, + float test_point_bearing, float test_point_distance); + + matrix::Vector2 + generateLoiterPointForFixedWing(geofence_violation_type_u violation_type, Geofence *geofence); + + float computeBrakingDistanceMultirotor(); + + float computeVerticalBrakingDistanceMultirotor(); + + matrix::Vector2 generateLoiterPointForMultirotor(geofence_violation_type_u violation_type, Geofence *geofence); + + float generateLoiterAltitudeForFixedWing(geofence_violation_type_u violation_type); + + float generateLoiterAltitudeForMulticopter(geofence_violation_type_u violation_type); + + float getMinHorDistToFenceMulticopter() {return _min_hor_dist_to_fence_mc;} + + float getMinVertDistToFenceMultirotor() {return _min_vert_dist_to_fence_mc;} + + void setTestPointBearing(float test_point_bearing) { _test_point_bearing = test_point_bearing; } + + void setHorizontalTestPointDistance(float test_point_distance) { _test_point_distance = test_point_distance; } + + void setVerticalTestPointDistance(float distance) { _vertical_test_point_distance = distance; } + + void setHorizontalVelocity(float velocity_hor_abs) { _velocity_hor_abs = velocity_hor_abs; } + + void setClimbRate(float climbrate) { _climbrate = climbrate; } + + void setCurrentPosition(double lat, double lon, float alt); + + void setHomePosition(double lat, double lon, float alt); + + void setMaxHorDistHome(float dist) { _max_hor_dist_home = dist; } + + void setMaxVerDistHome(float dist) { _max_ver_dist_home = dist; } + + void updateParameters(); + +private: + struct { + param_t param_mpc_jerk_max; + param_t param_mpc_acc_hor; + param_t param_mpc_acc_hor_max; + param_t param_mpc_jerk_auto; + param_t param_mpc_acc_up_max; + param_t param_mpc_acc_down_max; + + } _paramHandle; + + struct { + float param_mpc_jerk_max; + float param_mpc_acc_hor; + float param_mpc_acc_hor_max; + float param_mpc_jerk_auto; + float param_mpc_acc_up_max; + float param_mpc_acc_down_max; + + } _params; + + float _test_point_bearing{0.0f}; + float _test_point_distance{0.0f}; + float _vertical_test_point_distance{0.0f}; + float _velocity_hor_abs{0.0f}; + float _climbrate{0.0f}; + float _current_alt_amsl{0.0f}; + float _min_hor_dist_to_fence_mc{0.0f}; + float _min_vert_dist_to_fence_mc{0.0f}; + + float _multirotor_braking_distance{0.0f}; + float _multirotor_vertical_braking_distance{0.0f}; + + matrix::Vector2 _current_pos_lat_lon{}; + matrix::Vector2 _home_lat_lon {}; + float _home_alt_amsl{0.0f}; + + float _max_hor_dist_home{0.0f}; + float _max_ver_dist_home{0.0f}; + + void updateMinHorDistToFenceMultirotor(); + + void updateMinVertDistToFenceMultirotor(); + + matrix::Vector2 waypointFromHomeToTestPointAtDist(float distance); + +}; diff --git a/src/prometheus_px4/src/modules/navigator/RangeRTLTest.cpp b/src/prometheus_px4/src/modules/navigator/RangeRTLTest.cpp new file mode 100644 index 0000000..13efe03 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/RangeRTLTest.cpp @@ -0,0 +1,219 @@ +#define MODULE_NAME "Navigator" + +#include "navigator.h" +#include "rtl.h" + +#include + + +#include + +TEST(Navigator_and_RTL, interact_correctly) +{ + Navigator n; + RTL rtl(&n); + + + home_position_s home_pos{}; + home_pos.valid_hpos = true; + home_pos.valid_lpos = true; + home_pos.valid_alt = true; + home_pos.timestamp = 1000; + + vehicle_global_position_s glob_pos{}; + + vehicle_status_s v_status{}; + v_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + // TODO: can't do this, it hangs forever in Navigator's while loop + // uORB::Publication home_pos_pub{ORB_ID(home_position)}; + // uORB::Publication global_pos_pub{ORB_ID(vehicle_global_position)}; + // uORB::Publication local_pos_pub{ORB_ID(vehicle_local_position)}; + // uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; + // home_pos_pub.publish(home_pos); + // global_pos_pub.publish(glob_pos); + // local_pos_pub.publish(local_pos); + // vehicle_status_pub.publish(v_status); + // n.run(); + + // Hacky-hack, don't use pub-sub, just set them directly in navigator. NB! This isn't the "real" API, they should + // be set via pub-sub otherwise this will be a constant drag on development + *n.get_home_position() = home_pos; + *n.get_global_position() = glob_pos; + *n.get_vstatus() = v_status; + + uORB::SubscriptionData _rtl_flight_time_sub{ORB_ID(rtl_flight_time)}; + ASSERT_FALSE(_rtl_flight_time_sub.update()); + rtl.find_RTL_destination(); + ASSERT_TRUE(_rtl_flight_time_sub.update()); + auto msg = _rtl_flight_time_sub.get(); + EXPECT_EQ(msg.rtl_time_s, 0); + + // WHEN: we set the vehicle type to multirotor + v_status.vehicle_type = n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + float xy, z; + rtl.get_rtl_xy_z_speed(xy, z); + + // THEN: the RTL speed should correspond to multirotor parameters + float xy_desired, z_desired; + param_get(param_handle(px4::params::MPC_XY_CRUISE), &xy_desired); + param_get(param_handle(px4::params::MPC_Z_VEL_MAX_DN), &z_desired); + EXPECT_FLOAT_EQ(xy, xy_desired); + EXPECT_FLOAT_EQ(z, z_desired); + + // WHEN: it is a fixed wing + n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + rtl.get_rtl_xy_z_speed(xy, z); + + // THEN: it should be fixed wing parameters + param_get(param_handle(px4::params::FW_AIRSPD_TRIM), &xy_desired); + param_get(param_handle(px4::params::FW_T_SINK_MIN), &z_desired); + EXPECT_FLOAT_EQ(xy, xy_desired); + EXPECT_FLOAT_EQ(z, z_desired); + + // WHEN: it is rover + n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + rtl.get_rtl_xy_z_speed(xy, z); + + // THEN: it should be rover parameters, and z should just be large (no RTL time in Z -> high speed) + param_get(param_handle(px4::params::GND_SPEED_THR_SC), &xy_desired); + EXPECT_FLOAT_EQ(xy, xy_desired); + EXPECT_GT(z, 1000); +} + +class RangeRTL_tth : public ::testing::Test +{ +public: + matrix::Vector3f rtl_vector; + matrix::Vector3f rtl_point_local_pos; + matrix::Vector2f wind_vel; + float vehicle_speed; + float vehicle_descent_speed; + + void SetUp() override + { + rtl_vector = matrix::Vector3f(0, 0, 0); + rtl_point_local_pos = matrix::Vector3f(0, 0, 0); + wind_vel = matrix::Vector2f(0, 0); + vehicle_speed = 5; + vehicle_descent_speed = 1; + } +}; + +TEST_F(RangeRTL_tth, zero_distance_zero_time) +{ + // GIVEN: zero distances (defaults) + + // WHEN: we get the tth + float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be zero + EXPECT_FLOAT_EQ(tth, 0.f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy) +{ + // GIVEN: 10 seconds of distance + vehicle_speed = 6.2f; + rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); + + // WHEN: we get the tth + float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be ten seconds + EXPECT_FLOAT_EQ(tth, 10.f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy_5_seconds_z) +{ + // GIVEN: 10 seconds of xy distance and 5 seconds of Z + vehicle_speed = 4.2f; + vehicle_descent_speed = 1.2f; + rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); + rtl_vector(2) = vehicle_descent_speed * 5; + + // WHEN: we get the tth + float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be 15 seconds + EXPECT_FLOAT_EQ(tth, 15.f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy_downwind_to_home) +{ + // GIVEN: 10 seconds of xy distance and 5 seconds of Z, and the wind is towards home + vehicle_speed = 4.2f; + rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); + + wind_vel = matrix::Vector2f(-1, -1); + + // WHEN: we get the tth + float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be 10, because we don't rely on wind towards home for RTL + EXPECT_FLOAT_EQ(tth, 10.f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy_upwind_to_home) +{ + // GIVEN: 10 seconds of distance + vehicle_speed = 4.2f; + vehicle_descent_speed = 1.2f; + rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); + + wind_vel = matrix::Vector2f(1, 1) / sqrt(2) * vehicle_speed / 10; + + // WHEN: we get the tth + float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be 11.111111... because it slows us down by 10% and time = dist/speed + EXPECT_FLOAT_EQ(tth, 10 / 0.9f); +} + +TEST_F(RangeRTL_tth, ten_seconds_xy_z_wind_across_home) +{ + // GIVEN: a 3 4 5 triangle, with vehicle airspeed being 5, wind 3, ground speed 4 + // and the vehicle is 10 seconds away + + vehicle_speed = 5.f; + wind_vel = matrix::Vector2f(-1, 1) / sqrt(2) * 3.; + rtl_vector(0) = rtl_vector(1) = -(4 * 10) / sqrtf(2); + + // WHEN: we get the tth + float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should be 10 + EXPECT_FLOAT_EQ(tth, 10); +} + +TEST_F(RangeRTL_tth, too_strong_upwind_to_home) +{ + // GIVEN: 10 seconds of distance + vehicle_speed = 4.2f; + vehicle_descent_speed = 1.2f; + rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); + + wind_vel = matrix::Vector2f(1, 1) / sqrt(2) * vehicle_speed * 1.001f; + + // WHEN: we get the tth + float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should never get home + EXPECT_TRUE(std::isinf(tth)) << tth; +} + +TEST_F(RangeRTL_tth, too_strong_crosswind_to_home) +{ + // GIVEN: 10 seconds of distance + vehicle_speed = 4.2f; + vehicle_descent_speed = 1.2f; + rtl_vector(0) = rtl_vector(1) = -(vehicle_speed * 10) / sqrtf(2); + + wind_vel = matrix::Vector2f(1, -1) / sqrt(2) * vehicle_speed * 1.001f; + + // WHEN: we get the tth + float tth = time_to_home(rtl_vector, wind_vel, vehicle_speed, vehicle_descent_speed); + + // THEN: it should never get home + EXPECT_TRUE(std::isinf(tth)) << tth; +} diff --git a/src/prometheus_px4/src/modules/navigator/enginefailure.cpp b/src/prometheus_px4/src/modules/navigator/enginefailure.cpp new file mode 100644 index 0000000..dcec92c --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/enginefailure.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 enginefailure.cpp +* Helper class for a fixedwing engine failure mode +* +* @author Thomas Gubler +*/ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "navigator.h" +#include "enginefailure.h" + +EngineFailure::EngineFailure(Navigator *navigator) : + MissionBlock(navigator), + _ef_state(EF_STATE_NONE) +{ +} + +void +EngineFailure::on_inactive() +{ + _ef_state = EF_STATE_NONE; +} + +void +EngineFailure::on_activation() +{ + _ef_state = EF_STATE_NONE; + advance_ef(); + set_ef_item(); +} + +void +EngineFailure::on_active() +{ + if (is_mission_item_reached()) { + advance_ef(); + set_ef_item(); + } +} + +void +EngineFailure::set_ef_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + pos_sp_triplet->previous = pos_sp_triplet->current; + _navigator->set_can_loiter_at_sp(false); + + switch (_ef_state) { + case EF_STATE_LOITERDOWN: { + //XXX create mission item at ground (below?) here + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + //XXX setting altitude to a very low value, evaluate other options + _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +EngineFailure::advance_ef() +{ + switch (_ef_state) { + case EF_STATE_NONE: + mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down"); + _ef_state = EF_STATE_LOITERDOWN; + break; + + default: + break; + } +} diff --git a/src/prometheus_px4/src/modules/navigator/enginefailure.h b/src/prometheus_px4/src/modules/navigator/enginefailure.h new file mode 100644 index 0000000..8ba17d8 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/enginefailure.h @@ -0,0 +1,73 @@ +/*************************************************************************** + * + * 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 enginefailure.h + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class EngineFailure : public MissionBlock +{ +public: + EngineFailure(Navigator *navigator); + ~EngineFailure() = default; + + void on_inactive() override; + void on_activation() override; + void on_active() override; + +private: + enum EFState { + EF_STATE_NONE = 0, + EF_STATE_LOITERDOWN = 1, + } _ef_state{EF_STATE_NONE}; + + /** + * Set the DLL item + */ + void set_ef_item(); + + /** + * Move to next EF item + */ + void advance_ef(); + +}; diff --git a/src/prometheus_px4/src/modules/navigator/follow_target.cpp b/src/prometheus_px4/src/modules/navigator/follow_target.cpp new file mode 100644 index 0000000..8e94ab8 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/follow_target.cpp @@ -0,0 +1,407 @@ +/**************************************************************************** + * + * 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 followme.cpp + * + * Helper class to track and follow a given position + * + * @author Jimmy Johnson + */ + +#include "follow_target.h" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include "navigator.h" + +using namespace matrix; + +constexpr float FollowTarget::_follow_position_matricies[4][9]; + +FollowTarget::FollowTarget(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ + _current_vel.zero(); + _step_vel.zero(); + _est_target_vel.zero(); + _target_distance.zero(); + _target_position_offset.zero(); + _target_position_delta.zero(); +} + +void FollowTarget::on_inactive() +{ + reset_target_validity(); +} + +void FollowTarget::on_activation() +{ + _follow_offset = _param_nav_ft_dst.get() < 1.0F ? 1.0F : _param_nav_ft_dst.get(); + + _responsiveness = math::constrain((float) _param_nav_ft_rs.get(), .1F, 1.0F); + + _follow_target_position = _param_nav_ft_fs.get(); + + if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) { + _follow_target_position = FOLLOW_FROM_BEHIND; + } + + _rot_matrix = Dcmf(_follow_position_matricies[_follow_target_position]); +} + +void FollowTarget::on_active() +{ + struct map_projection_reference_s target_ref; + follow_target_s target_motion_with_offset = {}; + uint64_t current_time = hrt_absolute_time(); + bool radius_entered = false; + bool radius_exited = false; + bool updated = false; + float dt_ms = 0; + + if (_follow_target_sub.updated()) { + updated = true; + follow_target_s target_motion; + + _target_updates++; + + // save last known motion topic for interpolation later + + _previous_target_motion = _current_target_motion; + + _follow_target_sub.copy(&target_motion); + + if (_current_target_motion.timestamp == 0) { + _current_target_motion = target_motion; + } + + _current_target_motion = target_motion; + _current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)( + 1 - _responsiveness); + _current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)( + 1 - _responsiveness); + + } else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) { + reset_target_validity(); + } + + // update distance to target + + if (target_position_valid()) { + + // get distance to target + + map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), + &_target_distance(1)); + + } + + // update target velocity + + if (target_velocity_valid() && updated) { + + dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); + + // ignore a small dt + if (dt_ms > 10.0F) { + // get last gps known reference for target + map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon); + + // calculate distance the target has moved + map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, + &(_target_position_delta(0)), &(_target_position_delta(1))); + + // update the average velocity of the target based on the position + if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) { + // No need to estimate target velocity if we can take it from the target + _est_target_vel(0) = _current_target_motion.vx; + _est_target_vel(1) = _current_target_motion.vy; + _est_target_vel(2) = 0.0f; + + } else { + _est_target_vel = _target_position_delta / (dt_ms / 1000.0f); + } + + // if the target is moving add an offset and rotation + if (_est_target_vel.length() > .5F) { + _target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset; + } + + // are we within the target acceptance radius? + // give a buffer to exit/enter the radius to give the velocity controller + // a chance to catch up + + radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); + radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M); + + // to keep the velocity increase/decrease smooth + // calculate how many velocity increments/decrements + // it will take to reach the targets velocity + // with the given amount of steps also add a feed forward input that adjusts the + // velocity as the position gap increases since + // just traveling at the exact velocity of the target will not + // get any closer or farther from the target + + _step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K; + _step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS); + _step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS); + + // if we are less than 1 meter from the target don't worry about trying to yaw + // lock the yaw until we are at a distance that makes sense + + if ((_target_distance).length() > 1.0F) { + + // yaw rate smoothing + + // this really needs to control the yaw rate directly in the attitude pid controller + // but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode + + _yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _current_target_motion.lat, + _current_target_motion.lon); + + _yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f)); + + } else { + _yaw_angle = _yaw_rate = NAN; + } + } + +// warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d yaw rate = %3.6f", +// (double) _step_vel(0), +// (double) _step_vel(1), +// (double) _current_vel(0), +// (double) _current_vel(1), +// (double) _est_target_vel(0), +// (double) _est_target_vel(1), +// (double) (_target_distance).length(), +// (double) (_target_position_offset + _target_distance).length(), +// _follow_target_state, +// (double) _yaw_rate); + } + + if (target_position_valid()) { + + // get the target position using the calculated offset + + map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon); + map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), + &target_motion_with_offset.lat, &target_motion_with_offset.lon); + } + + // clamp yaw rate smoothing if we are with in + // 3 degrees of facing target + + if (PX4_ISFINITE(_yaw_rate)) { + if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) { + _yaw_rate = NAN; + } + } + + // update state machine + + switch (_follow_target_state) { + + case TRACK_POSITION: { + + if (radius_entered) { + _follow_target_state = TRACK_VELOCITY; + + } else if (target_velocity_valid()) { + set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle); + // keep the current velocity updated with the target velocity for when it's needed + _current_vel = _est_target_vel; + + update_position_sp(true, true, _yaw_rate); + + } else { + _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; + } + + break; + } + + case TRACK_VELOCITY: { + + if (radius_exited) { + _follow_target_state = TRACK_POSITION; + + } else if (target_velocity_valid()) { + + if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) { + _current_vel += _step_vel; + _last_update_time = current_time; + } + + set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle); + + update_position_sp(true, false, _yaw_rate); + + } else { + _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; + } + + break; + } + + case SET_WAIT_FOR_TARGET_POSITION: { + + // Climb to the minimum altitude + // and wait until a position is received + + follow_target_s target = {}; + + // for now set the target at the minimum height above the uav + + target.lat = _navigator->get_global_position()->lat; + target.lon = _navigator->get_global_position()->lon; + target.alt = 0.0F; + + set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target, _yaw_angle); + + update_position_sp(false, false, _yaw_rate); + + _follow_target_state = WAIT_FOR_TARGET_POSITION; + } + + /* FALLTHROUGH */ + + case WAIT_FOR_TARGET_POSITION: { + + if (is_mission_item_reached() && target_velocity_valid()) { + _target_position_offset(0) = _follow_offset; + _follow_target_state = TRACK_POSITION; + } + + break; + } + } +} + +void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate) +{ + // convert mission item to current setpoint + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // activate line following in pos control if position is valid + + pos_sp_triplet->previous.valid = use_position; + pos_sp_triplet->previous = pos_sp_triplet->current; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET; + pos_sp_triplet->current.velocity_valid = use_velocity; + pos_sp_triplet->current.vx = _current_vel(0); + pos_sp_triplet->current.vy = _current_vel(1); + pos_sp_triplet->next.valid = false; + pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate); + pos_sp_triplet->current.yawspeed = yaw_rate; + _navigator->set_position_setpoint_triplet_updated(); +} + +void FollowTarget::reset_target_validity() +{ + _yaw_rate = NAN; + _previous_target_motion = {}; + _current_target_motion = {}; + _target_updates = 0; + _current_vel.zero(); + _step_vel.zero(); + _est_target_vel.zero(); + _target_distance.zero(); + _target_position_offset.zero(); + reset_mission_item_reached(); + _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; +} + +bool FollowTarget::target_velocity_valid() +{ + // need at least 2 continuous data points for velocity estimate + return (_target_updates >= 2); +} + +bool FollowTarget::target_position_valid() +{ + // need at least 1 continuous data points for position estimate + return (_target_updates >= 1); +} + +void +FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, + float yaw) +{ + if (_navigator->get_land_detected()->landed) { + /* landed, don't takeoff, but switch to IDLE mode */ + item->nav_cmd = NAV_CMD_IDLE; + + } else { + + item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION; + + /* use current target position */ + item->lat = target.lat; + item->lon = target.lon; + item->altitude = _navigator->get_home_position()->alt; + + if (min_clearance > 8.0f) { + item->altitude += min_clearance; + + } else { + item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person) + } + } + + item->altitude_is_relative = false; + item->yaw = yaw; + item->loiter_radius = _navigator->get_loiter_radius(); + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; +} diff --git a/src/prometheus_px4/src/modules/navigator/follow_target.h b/src/prometheus_px4/src/modules/navigator/follow_target.h new file mode 100644 index 0000000..25086dc --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/follow_target.h @@ -0,0 +1,148 @@ +/*************************************************************************** + * + * Copyright (c) 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 followme.cpp + * + * Helper class to track and follow a given position + * + * @author Jimmy Johnson + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" + +#include +#include + +#include +#include +#include + +class FollowTarget : public MissionBlock, public ModuleParams +{ + +public: + FollowTarget(Navigator *navigator); + ~FollowTarget() = default; + + void on_inactive() override; + void on_activation() override; + void on_active() override; + +private: + + static constexpr int TARGET_TIMEOUT_MS = 2500; + static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5; + static constexpr int INTERPOLATION_PNTS = 20; + static constexpr float FF_K = .25F; + static constexpr float OFFSET_M = 8; + + enum FollowTargetState { + TRACK_POSITION, + TRACK_VELOCITY, + SET_WAIT_FOR_TARGET_POSITION, + WAIT_FOR_TARGET_POSITION + }; + + enum { + FOLLOW_FROM_RIGHT, + FOLLOW_FROM_BEHIND, + FOLLOW_FROM_FRONT, + FOLLOW_FROM_LEFT + }; + + static constexpr float _follow_position_matricies[4][9] = { + { 1.0F, -1.0F, 0.0F, 1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow right + {-1.0F, 0.0F, 0.0F, 0.0F, -1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow behind + { 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow front + { 1.0F, 1.0F, 0.0F, -1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F} // follow left side + }; + + DEFINE_PARAMETERS( + (ParamFloat) _param_nav_min_ft_ht, + (ParamFloat) _param_nav_ft_dst, + (ParamInt) _param_nav_ft_fs, + (ParamFloat) _param_nav_ft_rs + ) + + FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION}; + int _follow_target_position{FOLLOW_FROM_BEHIND}; + + uORB::Subscription _follow_target_sub{ORB_ID(follow_target)}; + float _step_time_in_ms{0.0f}; + float _follow_offset{OFFSET_M}; + + uint64_t _target_updates{0}; + uint64_t _last_update_time{0}; + + matrix::Vector3f _current_vel; + matrix::Vector3f _step_vel; + matrix::Vector3f _est_target_vel; + matrix::Vector3f _target_distance; + matrix::Vector3f _target_position_offset; + matrix::Vector3f _target_position_delta; + matrix::Vector3f _filtered_target_position_delta; + + follow_target_s _current_target_motion{}; + follow_target_s _previous_target_motion{}; + + float _yaw_rate{0.0f}; + float _responsiveness{0.0f}; + float _yaw_angle{0.0f}; + + // Mavlink defined motion reporting capabilities + enum { + POS = 0, + VEL = 1, + ACCEL = 2, + ATT_RATES = 3 + }; + + matrix::Dcmf _rot_matrix; + + void track_target_position(); + void track_target_velocity(); + bool target_velocity_valid(); + bool target_position_valid(); + void reset_target_validity(); + void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate); + void update_target_motion(); + void update_target_velocity(); + + /** + * Set follow_target item + */ + void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw); +}; diff --git a/src/prometheus_px4/src/modules/navigator/follow_target_params.c b/src/prometheus_px4/src/modules/navigator/follow_target_params.c new file mode 100644 index 0000000..4dc50bf --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/follow_target_params.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 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 follow_target_params.c + * + * Parameters for follow target mode + * + * @author Jimmy Johnson + */ + +/* + * Follow target parameters + */ + +/** + * Minimum follow target altitude + * + * The minimum height in meters relative to home for following a target + * + * @unit m + * @min 8.0 + * @group Follow target + */ +PARAM_DEFINE_FLOAT(NAV_MIN_FT_HT, 8.0f); + +/** + * Distance to follow target from + * + * The distance in meters to follow the target at + * + * @unit m + * @min 1.0 + * @group Follow target + */ +PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f); + +/** + * Side to follow target from + * + * The side to follow the target from (front right = 0, behind = 1, front = 2, front left = 3) + * + * @min 0 + * @max 3 + * @group Follow target + */ +PARAM_DEFINE_INT32(NAV_FT_FS, 1); + +/** + * Dynamic filtering algorithm responsiveness to target movement + * + * lower numbers increase the responsiveness to changing long lat + * but also ignore less noise + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @group Follow target + */ +PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.5f); + diff --git a/src/prometheus_px4/src/modules/navigator/geofence.cpp b/src/prometheus_px4/src/modules/navigator/geofence.cpp new file mode 100644 index 0000000..6f4d608 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/geofence.cpp @@ -0,0 +1,631 @@ +/**************************************************************************** + * + * Copyright (c) 2013,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 geofence.cpp + * Provides functions for handling the geofence + * + * @author Jean Cyr + * @author Thomas Gubler + * @author Beat Küng + */ +#include "geofence.h" +#include "navigator.h" + +#include + +#include +#include +#include +#include + +#include "navigator.h" + +#define GEOFENCE_RANGE_WARNING_LIMIT 5000000 + +Geofence::Geofence(Navigator *navigator) : + ModuleParams(navigator), + _navigator(navigator), + _sub_airdata(ORB_ID(vehicle_air_data)) +{ + // we assume there's no concurrent fence update on startup + if (_navigator != nullptr) { + _updateFence(); + } +} + +Geofence::~Geofence() +{ + if (_polygons) { + delete[](_polygons); + } +} + +void Geofence::updateFence() +{ + // Note: be aware that when calling this, it can block for quite some time, the duration of a geofence transfer. + // However this is currently not used + int ret = dm_lock(DM_KEY_FENCE_POINTS); + + if (ret != 0) { + PX4_ERR("lock failed"); + return; + } + + _updateFence(); + dm_unlock(DM_KEY_FENCE_POINTS); +} + +void Geofence::_updateFence() +{ + + // initialize fence points count + mission_stats_entry_s stats; + int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + int num_fence_items = 0; + + if (ret == sizeof(mission_stats_entry_s)) { + num_fence_items = stats.num_items; + _update_counter = stats.update_counter; + } + + // iterate over all polygons and store their starting vertices + _num_polygons = 0; + int current_seq = 1; + + while (current_seq <= num_fence_items) { + mission_fence_point_s mission_fence_point; + bool is_circle_area = false; + + if (dm_read(DM_KEY_FENCE_POINTS, current_seq, &mission_fence_point, sizeof(mission_fence_point_s)) != + sizeof(mission_fence_point_s)) { + PX4_ERR("dm_read failed"); + break; + } + + switch (mission_fence_point.nav_cmd) { + case NAV_CMD_FENCE_RETURN_POINT: + // TODO: do we need to store this? + ++current_seq; + break; + + case NAV_CMD_FENCE_CIRCLE_INCLUSION: + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: + is_circle_area = true; + + /* FALLTHROUGH */ + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: + if (!is_circle_area && mission_fence_point.vertex_count == 0) { + ++current_seq; // avoid endless loop + PX4_ERR("Polygon with 0 vertices. Skipping"); + + } else { + if (_polygons) { + // resize: this is somewhat inefficient, but we do not expect there to be many polygons + PolygonInfo *new_polygons = new PolygonInfo[_num_polygons + 1]; + + if (new_polygons) { + memcpy(new_polygons, _polygons, sizeof(PolygonInfo) * _num_polygons); + } + + delete[](_polygons); + _polygons = new_polygons; + + } else { + _polygons = new PolygonInfo[1]; + } + + if (!_polygons) { + _num_polygons = 0; + PX4_ERR("alloc failed"); + return; + } + + PolygonInfo &polygon = _polygons[_num_polygons]; + polygon.dataman_index = current_seq; + polygon.fence_type = mission_fence_point.nav_cmd; + + if (is_circle_area) { + polygon.circle_radius = mission_fence_point.circle_radius; + current_seq += 1; + + } else { + polygon.vertex_count = mission_fence_point.vertex_count; + current_seq += mission_fence_point.vertex_count; + } + + ++_num_polygons; + } + + break; + + default: + PX4_ERR("unhandled Fence command: %i", (int)mission_fence_point.nav_cmd); + ++current_seq; + break; + } + + } + +} + +bool Geofence::checkAll(const struct vehicle_global_position_s &global_position) +{ + return checkAll(global_position.lat, global_position.lon, global_position.alt); +} + +bool Geofence::checkAll(const struct vehicle_global_position_s &global_position, const float alt) +{ + return checkAll(global_position.lat, global_position.lon, alt); +} + +bool Geofence::check(const vehicle_global_position_s &global_position, const vehicle_gps_position_s &gps_position, + const home_position_s home_pos, bool home_position_set) +{ + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + return checkAll(global_position); + + } else { + return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (double)gps_position.alt * 1.0e-3); + } + + } else { + // get baro altitude + _sub_airdata.update(); + const float baro_altitude_amsl = _sub_airdata.get().baro_alt_meter; + + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + return checkAll(global_position, baro_altitude_amsl); + + } else { + return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, baro_altitude_amsl); + } + } +} + +bool Geofence::check(const struct mission_item_s &mission_item) +{ + return checkAll(mission_item.lat, mission_item.lon, mission_item.altitude); +} + +bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude) +{ + bool inside_fence = true; + + if (isHomeRequired() && _navigator->home_position_valid()) { + + const float max_horizontal_distance = _param_gf_max_hor_dist.get(); + + const double home_lat = _navigator->get_home_position()->lat; + const double home_lon = _navigator->get_home_position()->lon; + const float home_alt = _navigator->get_home_position()->alt; + + float dist_xy = -1.0f; + float dist_z = -1.0f; + + get_distance_to_point_global_wgs84(lat, lon, altitude, home_lat, home_lon, home_alt, &dist_xy, &dist_z); + + if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) { + if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)", + (double)max_horizontal_distance); + _last_horizontal_range_warning = hrt_absolute_time(); + } + + inside_fence = false; + } + } + + return inside_fence; +} + +bool Geofence::isBelowMaxAltitude(float altitude) +{ + bool inside_fence = true; + + if (isHomeRequired() && _navigator->home_position_valid()) { + + const float max_vertical_distance = _param_gf_max_ver_dist.get(); + const float home_alt = _navigator->get_home_position()->alt; + + float dist_z = altitude - home_alt; + + if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) { + if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)", + (double)max_vertical_distance); + _last_vertical_range_warning = hrt_absolute_time(); + } + + inside_fence = false; + } + } + + return inside_fence; +} + + +bool Geofence::checkAll(double lat, double lon, float altitude) +{ + bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude); + + inside_fence = inside_fence && isBelowMaxAltitude(altitude); + + // to be inside the geofence both fences have to report being inside + // as they both report being inside when not enabled + inside_fence = inside_fence && isInsidePolygonOrCircle(lat, lon, altitude); + + if (inside_fence) { + _outside_counter = 0; + return inside_fence; + + } else { + _outside_counter++; + + if (_outside_counter > _param_gf_count.get()) { + return inside_fence; + + } else { + return true; + } + } +} + +bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) +{ + // the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means + // the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now + if (dm_trylock(DM_KEY_FENCE_POINTS) != 0) { + return true; + } + + // we got the lock, now check if the fence data got updated + mission_stats_entry_s stats; + int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + + if (ret == sizeof(mission_stats_entry_s) && _update_counter != stats.update_counter) { + _updateFence(); + } + + if (isEmpty()) { + dm_unlock(DM_KEY_FENCE_POINTS); + /* Empty fence -> accept all points */ + return true; + } + + /* Vertical check */ + if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly + if (altitude > _altitude_max || altitude < _altitude_min) { + dm_unlock(DM_KEY_FENCE_POINTS); + return false; + } + } + + + /* Horizontal check: iterate all polygons & circles */ + bool outside_exclusion = true; + bool inside_inclusion = false; + bool had_inclusion_areas = false; + + for (int polygon_idx = 0; polygon_idx < _num_polygons; ++polygon_idx) { + if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + bool inside = insideCircle(_polygons[polygon_idx], lat, lon, altitude); + + if (inside) { + inside_inclusion = true; + } + + had_inclusion_areas = true; + + } else if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + bool inside = insideCircle(_polygons[polygon_idx], lat, lon, altitude); + + if (inside) { + outside_exclusion = false; + } + + } else { // it's a polygon + bool inside = insidePolygon(_polygons[polygon_idx], lat, lon, altitude); + + if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + if (inside) { + inside_inclusion = true; + } + + had_inclusion_areas = true; + + } else { // exclusion + if (inside) { + outside_exclusion = false; + } + } + } + } + + dm_unlock(DM_KEY_FENCE_POINTS); + + return (!had_inclusion_areas || inside_inclusion) && outside_exclusion; +} + +bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude) +{ + + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) + * Only supports non-complex polygons (not self intersecting) + */ + + mission_fence_point_s temp_vertex_i; + mission_fence_point_s temp_vertex_j; + bool c = false; + + for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) { + if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, &temp_vertex_i, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + break; + } + + if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, &temp_vertex_j, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + break; + } + + if (temp_vertex_i.frame != NAV_FRAME_GLOBAL && temp_vertex_i.frame != NAV_FRAME_GLOBAL_INT + && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT + && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + // TODO: handle different frames + PX4_ERR("Frame type %i not supported", (int)temp_vertex_i.frame); + break; + } + + if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { + c = !c; + } + } + + return c; +} + +bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude) +{ + + mission_fence_point_s circle_point; + + if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index, &circle_point, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + PX4_ERR("dm_read failed"); + return false; + } + + if (circle_point.frame != NAV_FRAME_GLOBAL && circle_point.frame != NAV_FRAME_GLOBAL_INT + && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT + && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + // TODO: handle different frames + PX4_ERR("Frame type %i not supported", (int)circle_point.frame); + return false; + } + + if (!map_projection_initialized(&_projection_reference)) { + map_projection_init(&_projection_reference, lat, lon); + } + + float x1, y1, x2, y2; + map_projection_project(&_projection_reference, lat, lon, &x1, &y1); + map_projection_project(&_projection_reference, circle_point.lat, circle_point.lon, &x2, &y2); + float dx = x1 - x2, dy = y1 - y2; + return dx * dx + dy * dy < circle_point.circle_radius * circle_point.circle_radius; +} + +bool +Geofence::valid() +{ + return true; // always valid +} + +int +Geofence::loadFromFile(const char *filename) +{ + FILE *fp; + char line[120]; + int pointCounter = 0; + bool gotVertical = false; + const char commentChar = '#'; + int rc = PX4_ERROR; + + /* Make sure no data is left in the datamanager */ + clearDm(); + + /* open the mixer definition file */ + fp = fopen(GEOFENCE_FILENAME, "r"); + + if (fp == nullptr) { + return PX4_ERROR; + } + + /* create geofence points from valid lines and store in DM */ + for (;;) { + /* get a line, bail on error/EOF */ + if (fgets(line, sizeof(line), fp) == nullptr) { + break; + } + + /* Trim leading whitespace */ + size_t textStart = 0; + + while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; } + + /* if the line starts with #, skip */ + if (line[textStart] == commentChar) { + continue; + } + + /* if there is only a linefeed, skip it */ + if (line[0] == '\n') { + continue; + } + + if (gotVertical) { + /* Parse the line as a geofence point */ + mission_fence_point_s vertex; + vertex.frame = NAV_FRAME_GLOBAL; + vertex.nav_cmd = NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION; + vertex.vertex_count = 0; // this will be filled in a second pass + vertex.alt = 0; // alt is not used + + /* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */ + if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') { + /* Handle degree minute second format */ + double lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; + + if (sscanf(line, "DMS %lf %lf %lf %lf %lf %lf", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { + PX4_ERR("Scanf to parse DMS geofence vertex failed."); + goto error; + } + +// PX4_INFO("Geofence DMS: %.5lf %.5lf %.5lf ; %.5lf %.5lf %.5lf", lat_d, lat_m, lat_s, lon_d, lon_m, lon_s); + + vertex.lat = lat_d + lat_m / 60.0 + lat_s / 3600.0; + vertex.lon = lon_d + lon_m / 60.0 + lon_s / 3600.0; + + } else { + /* Handle decimal degree format */ + if (sscanf(line, "%lf %lf", &vertex.lat, &vertex.lon) != 2) { + PX4_ERR("Scanf to parse geofence vertex failed."); + goto error; + } + } + + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, DM_PERSIST_POWER_ON_RESET, &vertex, + sizeof(vertex)) != sizeof(vertex)) { + goto error; + } + + PX4_INFO("Geofence: point: %d, lat %.5lf: lon: %.5lf", pointCounter, vertex.lat, vertex.lon); + + pointCounter++; + + } else { + /* Parse the line as the vertical limits */ + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { + goto error; + } + + PX4_INFO("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); + gotVertical = true; + } + } + + + /* Check if import was successful */ + if (gotVertical && pointCounter > 2) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported"); + rc = PX4_OK; + + /* do a second pass, now that we know the number of vertices */ + for (int seq = 1; seq <= pointCounter; ++seq) { + mission_fence_point_s mission_fence_point; + + if (dm_read(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)) == + sizeof(mission_fence_point_s)) { + mission_fence_point.vertex_count = pointCounter; + dm_write(DM_KEY_FENCE_POINTS, seq, DM_PERSIST_POWER_ON_RESET, &mission_fence_point, + sizeof(mission_fence_point_s)); + } + } + + mission_stats_entry_s stats; + stats.num_items = pointCounter; + rc = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + + } else { + PX4_ERR("Geofence: import error"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence import error"); + } + + updateFence(); + +error: + fclose(fp); + return rc; +} + +int Geofence::clearDm() +{ + dm_clear(DM_KEY_FENCE_POINTS); + updateFence(); + return PX4_OK; +} + +bool Geofence::isHomeRequired() +{ + bool max_horizontal_enabled = (_param_gf_max_hor_dist.get() > FLT_EPSILON); + bool max_vertical_enabled = (_param_gf_max_ver_dist.get() > FLT_EPSILON); + bool geofence_action_rtl = (getGeofenceAction() == geofence_result_s::GF_ACTION_RTL); + + return max_horizontal_enabled || max_vertical_enabled || geofence_action_rtl; +} + +void Geofence::printStatus() +{ + int num_inclusion_polygons = 0, num_exclusion_polygons = 0, total_num_vertices = 0; + int num_inclusion_circles = 0, num_exclusion_circles = 0; + + for (int i = 0; i < _num_polygons; ++i) { + total_num_vertices += _polygons[i].vertex_count; + + if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + ++num_inclusion_polygons; + } + + if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + ++num_exclusion_polygons; + } + + if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + ++num_inclusion_circles; + } + + if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + ++num_exclusion_circles; + } + } + + PX4_INFO("Geofence: %i inclusion, %i exclusion polygons, %i inclusion, %i exclusion circles, %i total vertices", + num_inclusion_polygons, num_exclusion_polygons, num_inclusion_circles, num_exclusion_circles, + total_num_vertices); +} diff --git a/src/prometheus_px4/src/modules/navigator/geofence.h b/src/prometheus_px4/src/modules/navigator/geofence.h new file mode 100644 index 0000000..34a5ff6 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/geofence.h @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * 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 geofence.h + * Provides functions for handling the geofence + * + * @author Jean Cyr + * @author Thomas Gubler + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define GEOFENCE_FILENAME PX4_STORAGEDIR"/etc/geofence.txt" + +class Navigator; + +class Geofence : public ModuleParams +{ +public: + Geofence(Navigator *navigator); + Geofence(const Geofence &) = delete; + Geofence &operator=(const Geofence &) = delete; + virtual ~Geofence(); + + /* Altitude mode, corresponding to the param GF_ALTMODE */ + enum { + GF_ALT_MODE_WGS84 = 0, + GF_ALT_MODE_AMSL = 1 + }; + + /* Source, corresponding to the param GF_SOURCE */ + enum { + GF_SOURCE_GLOBALPOS = 0, + GF_SOURCE_GPS = 1 + }; + + /** + * update the geofence from dataman. + * It's generally not necessary to call this as it will automatically update when the data is changed. + */ + void updateFence(); + + /** + * Return whether the system obeys the geofence. + * + * @return true: system is obeying fence, false: system is violating fence + */ + bool check(const vehicle_global_position_s &global_position, + const vehicle_gps_position_s &gps_position, const home_position_s home_pos, bool home_position_set); + + /** + * Return whether a mission item obeys the geofence. + * + * @return true: system is obeying fence, false: system is violating fence + */ + bool check(const struct mission_item_s &mission_item); + + + bool isCloserThanMaxDistToHome(double lat, double lon, float altitude); + + bool isBelowMaxAltitude(float altitude); + + virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude); + + int clearDm(); + + bool valid(); + + /** + * Load a single inclusion polygon, replacing any already existing polygons. + * The file has one of the following formats: + * - Decimal Degrees: + * 0 900 + * 47.475273548913222 8.52672100067138672 + * 47.4608261578541359 8.53414535522460938 + * 47.4613484218861217 8.56444358825683594 + * 47.4830758091035534 8.53470325469970703 + * + * - Degree-Minute-Second: + * 0 900 + * DMS -26 -34 -10.4304 151 50 14.5428 + * DMS -26 -34 -11.8416 151 50 21.8580 + * DMS -26 -34 -36.5628 151 50 28.1112 + * DMS -26 -34 -37.1640 151 50 24.1620 + * + * Where the first line is min, max altitude in meters AMSL. + */ + int loadFromFile(const char *filename); + + bool isEmpty() { return _num_polygons == 0; } + + int getAltitudeMode() { return _param_gf_altmode.get(); } + int getSource() { return _param_gf_source.get(); } + int getGeofenceAction() { return _param_gf_action.get(); } + float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); } + float getMaxVerDistanceHome() { return _param_gf_max_ver_dist.get(); } + + bool isHomeRequired(); + + /** + * Check if a point passes the Geofence test. + * In addition to checkPolygons(), this takes all additional parameters into account. + * + * @return false for a geofence violation + */ + bool checkAll(double lat, double lon, float altitude); + + /** + * print Geofence status to the console + */ + void printStatus(); + +private: + Navigator *_navigator{nullptr}; + + hrt_abstime _last_horizontal_range_warning{0}; + hrt_abstime _last_vertical_range_warning{0}; + + float _altitude_min{0.0f}; + float _altitude_max{0.0f}; + + struct PolygonInfo { + uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region) + uint16_t dataman_index; + union { + uint16_t vertex_count; + float circle_radius; + }; + }; + PolygonInfo *_polygons{nullptr}; + int _num_polygons{0}; + + map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] + + DEFINE_PARAMETERS( + (ParamInt) _param_gf_action, + (ParamInt) _param_gf_altmode, + (ParamInt) _param_gf_source, + (ParamInt) _param_gf_count, + (ParamFloat) _param_gf_max_hor_dist, + (ParamFloat) _param_gf_max_ver_dist + ) + + uORB::SubscriptionData _sub_airdata; + + int _outside_counter{0}; + uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated + + /** + * implementation of updateFence(), but without locking + */ + void _updateFence(); + + /** + * Check if a point passes the Geofence test. + * This takes all polygons and minimum & maximum altitude into account + * + * The check passes if: (inside(polygon_inclusion_1) || inside(polygon_inclusion_2) || ... ) && + * !inside(polygon_exclusion_1) && !inside(polygon_exclusion_2) && ... + * && (altitude within [min, max]) + * or: no polygon configured + * @return result of the check above (false for a geofence violation) + */ + bool checkPolygons(double lat, double lon, float altitude); + + + + bool checkAll(const vehicle_global_position_s &global_position); + bool checkAll(const vehicle_global_position_s &global_position, float baro_altitude_amsl); + + /** + * Check if a single point is within a polygon + * @return true if within polygon + */ + bool insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude); + + /** + * Check if a single point is within a circle + * @param polygon must be a circle! + * @return true if within polygon the circle + */ + bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude); +}; diff --git a/src/prometheus_px4/src/modules/navigator/geofence_params.c b/src/prometheus_px4/src/modules/navigator/geofence_params.c new file mode 100644 index 0000000..42655d9 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/geofence_params.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 geofence_params.c + * + * Parameters for geofence + * + * @author Thomas Gubler + */ + +/* + * Geofence parameters, accessible via MAVLink + */ + +/** + * Geofence violation action. + * + * Note: Setting this value to 4 enables flight termination, + * which will kill the vehicle on violation of the fence. + * Due to the inherent danger of this, this function is + * disabled using a software circuit breaker, which needs + * to be reset to 0 to really shut down the system. + * + * @min 0 + * @max 5 + * @value 0 None + * @value 1 Warning + * @value 2 Hold mode + * @value 3 Return mode + * @value 4 Terminate + * @value 5 Land mode + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_ACTION, 2); + +/** + * Geofence altitude mode + * + * Select which altitude (AMSL) source should be used for geofence calculations. + * + * @min 0 + * @max 1 + * @value 0 Autopilot estimator global position altitude (GPS) + * @value 1 Raw barometer altitude (assuming standard atmospheric pressure) + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_ALTMODE, 0); + +/** + * Geofence source + * + * Select which position source should be used. Selecting GPS instead of global position makes sure that there is + * no dependence on the position estimator + * 0 = global position, 1 = GPS + * + * @min 0 + * @max 1 + * @value 0 GPOS + * @value 1 GPS + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_SOURCE, 0); + +/** + * Geofence counter limit + * + * Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered + * + * @min -1 + * @max 10 + * @increment 1 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_COUNT, -1); + +/** + * Max horizontal distance in meters. + * + * Maximum horizontal distance in meters the vehicle can be from home before triggering a geofence action. Disabled if 0. + * + * @unit m + * @min 0 + * @max 10000 + * @increment 1 + * @group Geofence + */ +PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0); + +/** + * Max vertical distance in meters. + * + * Maximum vertical distance in meters the vehicle can be from home before triggering a geofence action. Disabled if 0. + * + * @unit m + * @min 0 + * @max 10000 + * @increment 1 + * @group Geofence + */ +PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0); diff --git a/src/prometheus_px4/src/modules/navigator/gpsfailure.cpp b/src/prometheus_px4/src/modules/navigator/gpsfailure.cpp new file mode 100644 index 0000000..a475ae5 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/gpsfailure.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * 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 gpsfailure.cpp + * Helper class for gpsfailure mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include "gpsfailure.h" +#include "navigator.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; + +GpsFailure::GpsFailure(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ +} + +void +GpsFailure::on_inactive() +{ + /* reset GPSF state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _gpsf_state = GPSF_STATE_NONE; + } +} + +void +GpsFailure::on_activation() +{ + _gpsf_state = GPSF_STATE_NONE; + _timestamp_activation = hrt_absolute_time(); + advance_gpsf(); + set_gpsf_item(); +} + +void +GpsFailure::on_active() +{ + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + /* Position controller does not run in this mode: + * navigator has to publish an attitude setpoint */ + vehicle_attitude_setpoint_s att_sp = {}; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); + att_sp.pitch_body = math::radians(_param_nav_gpsf_p.get()); + att_sp.thrust_body[0] = _param_nav_gpsf_tr.get(); + + Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f)); + q.copyTo(att_sp.q_d); + + if (_navigator->get_vstatus()->is_vtol) { + _fw_virtual_att_sp_pub.publish(att_sp); + + } else { + _att_sp_pub.publish(att_sp); + + } + + /* Measure time */ + if ((_param_nav_gpsf_lt.get() > FLT_EPSILON) && + (hrt_elapsed_time(&_timestamp_activation) > _param_nav_gpsf_lt.get() * 1e6f)) { + /* no recovery, advance the state machine */ + PX4_WARN("GPS not recovered, switching to next failure state"); + advance_gpsf(); + } + + break; + } + + case GPSF_STATE_TERMINATE: + set_gpsf_item(); + advance_gpsf(); + break; + + default: + break; + } +} + +void +GpsFailure::set_gpsf_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Set pos sp triplet to invalid to stop pos controller */ + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + + switch (_gpsf_state) { + case GPSF_STATE_TERMINATE: { + /* Request flight termination from commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->set_mission_result_updated(); + PX4_WARN("GPS failure: request flight termination"); + } + break; + + default: + break; + } + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +GpsFailure::advance_gpsf() +{ + switch (_gpsf_state) { + case GPSF_STATE_NONE: + _gpsf_state = GPSF_STATE_LOITER; + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: loitering"); + break; + + case GPSF_STATE_LOITER: + _gpsf_state = GPSF_STATE_TERMINATE; + mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight"); + break; + + case GPSF_STATE_TERMINATE: + PX4_WARN("terminate"); + _gpsf_state = GPSF_STATE_END; + break; + + default: + break; + } +} diff --git a/src/prometheus_px4/src/modules/navigator/gpsfailure.h b/src/prometheus_px4/src/modules/navigator/gpsfailure.h new file mode 100644 index 0000000..bfe56a7 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/gpsfailure.h @@ -0,0 +1,90 @@ +/*************************************************************************** + * + * 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 gpsfailure.h + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#pragma once + +#include + +#include "mission_block.h" + +#include +#include + +class Navigator; + +class GpsFailure : public MissionBlock, public ModuleParams +{ +public: + GpsFailure(Navigator *navigator); + ~GpsFailure() = default; + + void on_inactive() override; + void on_activation() override; + void on_active() override; + +private: + DEFINE_PARAMETERS( + (ParamFloat) _param_nav_gpsf_lt, + (ParamFloat) _param_nav_gpsf_r, + (ParamFloat) _param_nav_gpsf_p, + (ParamFloat) _param_nav_gpsf_tr + ) + + enum GPSFState { + GPSF_STATE_NONE = 0, + GPSF_STATE_LOITER = 1, + GPSF_STATE_TERMINATE = 2, + GPSF_STATE_END = 3, + } _gpsf_state{GPSF_STATE_NONE}; + + hrt_abstime _timestamp_activation{0}; //*< timestamp when this mode was activated */ + + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)}; + /** + * Set the GPSF item + */ + void set_gpsf_item(); + + /** + * Move to next GPSF item + */ + void advance_gpsf(); + +}; diff --git a/src/prometheus_px4/src/modules/navigator/gpsfailure_params.c b/src/prometheus_px4/src/modules/navigator/gpsfailure_params.c new file mode 100644 index 0000000..d671308 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/gpsfailure_params.c @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 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 gpsfailure_params.c + * + * Parameters for GPSF navigation mode + * + */ + +/** + * Loiter time + * + * The time in seconds the system should do open loop loiter and wait for GPS recovery + * before it goes into flight termination. Set to 0 to disable. + * + * @unit s + * @min 0.0 + * @max 3600.0 + * @decimal 0 + * @increment 1 + * @group GPS Failure Navigation + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 0.0f); + +/** + * Fixed bank angle + * + * Roll in degrees during the loiter + * + * @unit deg + * @min 0.0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group GPS Failure Navigation + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); + +/** + * Fixed pitch angle + * + * Pitch in degrees during the open loop loiter + * + * @unit deg + * @min -30.0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group GPS Failure Navigation + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); + +/** + * Thrust + * + * Thrust value which is set during the open loop loiter + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group GPS Failure Navigation + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.0f); diff --git a/src/prometheus_px4/src/modules/navigator/land.cpp b/src/prometheus_px4/src/modules/navigator/land.cpp new file mode 100644 index 0000000..7a17c8c --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/land.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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 land.cpp + * + * Helper class to land at the current position + * + * @author Andreas Antener + */ + +#include "land.h" +#include "navigator.h" + +Land::Land(Navigator *navigator) : + MissionBlock(navigator) +{ +} + +void +Land::on_activation() +{ + /* set current mission item to Land */ + set_land_item(&_mission_item, true); + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(false); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Land::on_active() +{ + /* for VTOL update landing location during back transition */ + if (_navigator->get_vstatus()->is_vtol && + _navigator->get_vstatus()->in_transition_mode) { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + _navigator->set_position_setpoint_triplet_updated(); + } + + + if (_navigator->get_land_detected()->landed) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + set_idle_item(&_mission_item); + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + } +} diff --git a/src/prometheus_px4/src/modules/navigator/land.h b/src/prometheus_px4/src/modules/navigator/land.h new file mode 100644 index 0000000..fdbc2bf --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/land.h @@ -0,0 +1,54 @@ +/*************************************************************************** + * + * Copyright (c) 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 land.h + * + * Helper class to land at the current position + * + * @author Andreas Antener + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" + +class Land : public MissionBlock +{ +public: + Land(Navigator *navigator); + ~Land() = default; + + void on_activation() override; + void on_active() override; +}; diff --git a/src/prometheus_px4/src/modules/navigator/loiter.cpp b/src/prometheus_px4/src/modules/navigator/loiter.cpp new file mode 100644 index 0000000..fdfb0b0 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/loiter.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * 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 loiter.cpp + * + * Helper class to loiter + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "loiter.h" +#include "navigator.h" + +Loiter::Loiter(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ +} + +void +Loiter::on_inactive() +{ + _loiter_pos_set = false; +} + +void +Loiter::on_activation() +{ + if (_navigator->get_reposition_triplet()->current.valid) { + reposition(); + + } else { + set_loiter_position(); + } +} + +void +Loiter::on_active() +{ + if (_navigator->get_reposition_triplet()->current.valid) { + reposition(); + } + + // reset the loiter position if we get disarmed + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + _loiter_pos_set = false; + } +} + +void +Loiter::set_loiter_position() +{ + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && + _navigator->get_land_detected()->landed) { + + // Not setting loiter position if disarmed and landed, instead mark the current + // setpoint as invalid and idle (both, just to be sure). + + _navigator->set_can_loiter_at_sp(false); + _navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; + _navigator->set_position_setpoint_triplet_updated(); + _loiter_pos_set = false; + return; + + } else if (_loiter_pos_set) { + // Already set, nothing to do. + return; + } + + _loiter_pos_set = true; + + // set current mission item to loiter + set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt()); + + // convert mission item to current setpoint + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->current.velocity_valid = false; + pos_sp_triplet->previous.valid = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Loiter::reposition() +{ + // we can't reposition if we are not armed yet + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + return; + } + + struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet(); + + if (rep->current.valid) { + // set loiter position based on reposition command + + // convert mission item to current setpoint + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->current.velocity_valid = false; + pos_sp_triplet->previous.yaw = _navigator->get_local_position()->heading; + pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt; + memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current)); + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + _navigator->set_position_setpoint_triplet_updated(); + + // mark this as done + memset(rep, 0, sizeof(*rep)); + } +} diff --git a/src/prometheus_px4/src/modules/navigator/loiter.h b/src/prometheus_px4/src/modules/navigator/loiter.h new file mode 100644 index 0000000..9d2e84c --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/loiter.h @@ -0,0 +1,71 @@ +/*************************************************************************** + * + * Copyright (c) 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 loiter.h + * + * Helper class to loiter + * + * @author Julian Oes + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" + +#include + +class Loiter : public MissionBlock, public ModuleParams +{ +public: + Loiter(Navigator *navigator); + ~Loiter() = default; + + void on_inactive() override; + void on_activation() override; + void on_active() override; + +private: + /** + * Use the stored reposition location of the navigator + * to move to a new location. + */ + void reposition(); + + /** + * Set the position to hold based on the current local position + */ + void set_loiter_position(); + + bool _loiter_pos_set{false}; +}; diff --git a/src/prometheus_px4/src/modules/navigator/mission.cpp b/src/prometheus_px4/src/modules/navigator/mission.cpp new file mode 100644 index 0000000..dac7161 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/mission.cpp @@ -0,0 +1,1867 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018, 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 navigator_mission.cpp + * + * Helper class to access missions + * + * @author Julian Oes + * @author Thomas Gubler + * @author Anton Babushkin + * @author Ban Siesta + * @author Simon Wilks + * @author Andreas Antener + * @author Sander Smeets + * @author Lorenz Meier + */ + +#include "mission.h" +#include "navigator.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +Mission::Mission(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ +} + +void +Mission::on_inactive() +{ + /* We need to reset the mission cruising speed, otherwise the + * mission velocity which might have been set using mission items + * is used for missions such as RTL. */ + _navigator->set_cruising_speed(); + + // if we were executing an landing but have been inactive for 2 seconds, then make the landing invalid + // this prevents RTL to just continue at the current mission index + if (_navigator->getMissionLandingInProgress() && (hrt_absolute_time() - _time_mission_deactivated) > 2_s) { + _navigator->setMissionLandingInProgress(false); + } + + /* Without home a mission can't be valid yet anyway, let's wait. */ + if (!_navigator->home_position_valid()) { + return; + } + + if (_inited) { + if (_mission_sub.updated()) { + update_mission(); + + if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) { + _mission_type = MISSION_TYPE_MISSION; + } + } + + /* reset the current mission if needed */ + if (need_to_reset_mission()) { + reset_mission(_mission); + update_mission(); + _navigator->reset_cruising_speed(); + } + + } else { + + /* load missions from storage */ + mission_s mission_state = {}; + + dm_lock(DM_KEY_MISSION_STATE); + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + dm_unlock(DM_KEY_MISSION_STATE); + + if (read_res == sizeof(mission_s)) { + _mission.dataman_id = mission_state.dataman_id; + _mission.count = mission_state.count; + _current_mission_index = mission_state.current_seq; + + // find and store landing start marker (if available) + find_mission_land_start(); + } + + /* On init let's check the mission, maybe there is already one available. */ + check_mission_valid(false); + + _inited = true; + } + + /* require takeoff after non-loiter or landing */ + if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) { + _need_takeoff = true; + } + + /* reset so current mission item gets restarted if mission was paused */ + _work_item_type = WORK_ITEM_TYPE_DEFAULT; + + /* reset so MISSION_ITEM_REACHED isn't published */ + _navigator->get_mission_result()->seq_reached = -1; +} + +void +Mission::on_inactivation() +{ + // Disable camera trigger + vehicle_command_s cmd {}; + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; + // Pause trigger + cmd.param1 = -1.0f; + cmd.param3 = 1.0f; + _navigator->publish_vehicle_cmd(&cmd); + + _navigator->release_gimbal_control(); + + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } + + _time_mission_deactivated = hrt_absolute_time(); + + /* reset so current mission item gets restarted if mission was paused */ + _work_item_type = WORK_ITEM_TYPE_DEFAULT; +} + +void +Mission::on_activation() +{ + if (_mission_waypoints_changed) { + // do not set the closest mission item in the normal mission mode + if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) { + _current_mission_index = index_closest_mission_item(); + } + + _mission_waypoints_changed = false; + } + + // we already reset the mission items + _execution_mode_changed = false; + + set_mission_items(); + + // unpause triggering if it was paused + vehicle_command_s cmd = {}; + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; + // unpause trigger + cmd.param1 = -1.0f; + cmd.param3 = 0.0f; + _navigator->publish_vehicle_cmd(&cmd); +} + +void +Mission::on_active() +{ + check_mission_valid(false); + + /* check if anything has changed */ + bool mission_sub_updated = _mission_sub.updated(); + + if (mission_sub_updated) { + _navigator->reset_triplets(); + update_mission(); + } + + /* mission is running (and we are armed), need reset after disarm */ + _need_mission_reset = true; + + _mission_changed = false; + + /* reset mission items if needed */ + if (mission_sub_updated || _mission_waypoints_changed || _execution_mode_changed) { + if (_mission_waypoints_changed) { + // do not set the closest mission item in the normal mission mode + if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) { + _current_mission_index = index_closest_mission_item(); + } + + _mission_waypoints_changed = false; + } + + _execution_mode_changed = false; + set_mission_items(); + } + + /* lets check if we reached the current mission item */ + if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + /* If we just completed a takeoff which was inserted before the right waypoint, + there is no need to report that we reached it because we didn't. */ + if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) { + set_mission_item_reached(); + } + + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + } + + } else { + /* if waypoint position reached allow loiter on the setpoint */ + if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { + _navigator->set_can_loiter_at_sp(true); + } + } + + /* check if a cruise speed change has been commanded */ + if (_mission_type != MISSION_TYPE_NONE) { + cruising_speed_sp_update(); + } + + /* see if we need to update the current yaw heading */ + if (!_param_mis_mnt_yaw_ctl.get() + && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) + && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + || _mission_item.nav_cmd == NAV_CMD_LAND + || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND + || _work_item_type == WORK_ITEM_TYPE_ALIGN)) { + // Mount control is disabled If the vehicle is in ROI-mode, the vehicle + // needs to rotate such that ROI is in the field of view. + // ROI only makes sense for multicopters. + heading_sp_update(); + } + + // TODO: Add vtol heading update method if required. + // Question: Why does vtol ever have to update heading? + + /* check if landing needs to be aborted */ + if ((_mission_item.nav_cmd == NAV_CMD_LAND) + && (_navigator->abort_landing())) { + + do_abort_landing(); + } + + if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) { + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +bool +Mission::set_current_mission_index(uint16_t index) +{ + if (_navigator->get_mission_result()->valid && + (index != _current_mission_index) && (index < _mission.count)) { + + _current_mission_index = index; + + // a mission index is set manually which has the higher priority than the closest mission item + // as it is set by the user + _mission_waypoints_changed = false; + + // update mission items if already in active mission + if (_navigator->is_planned_mission()) { + // prevent following "previous - current" line + _navigator->get_position_setpoint_triplet()->previous.valid = false; + _navigator->get_position_setpoint_triplet()->current.valid = false; + _navigator->get_position_setpoint_triplet()->next.valid = false; + set_mission_items(); + } + + return true; + } + + return false; +} + +void +Mission::set_closest_item_as_current() +{ + _current_mission_index = index_closest_mission_item(); +} + +void +Mission::set_execution_mode(const uint8_t mode) +{ + if (_mission_execution_mode != mode) { + _execution_mode_changed = true; + _navigator->get_mission_result()->execution_mode = mode; + + + switch (_mission_execution_mode) { + case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: + case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: + if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { + // command a transition if in vtol mc mode + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _navigator->get_vstatus()->is_vtol && + !_navigator->get_land_detected()->landed) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous = pos_sp_triplet->current; + generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw); + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + issue_command(_mission_item); + } + + if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) { + _mission_type = MISSION_TYPE_MISSION; + } + + if (_current_mission_index > _mission.count - 1) { + _current_mission_index = _mission.count - 1; + + } else if (_current_mission_index > 0) { + --_current_mission_index; + } + + _work_item_type = WORK_ITEM_TYPE_DEFAULT; + } + + break; + + case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: + if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) || + (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) { + // handle switch from reverse to forward mission + if (_current_mission_index < 0) { + _current_mission_index = 0; + + } else if (_current_mission_index < _mission.count - 1) { + ++_current_mission_index; + } + + _work_item_type = WORK_ITEM_TYPE_DEFAULT; + } + + break; + + } + + _mission_execution_mode = mode; + } +} + +bool +Mission::find_mission_land_start() +{ + /* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index + * return false if not found + */ + + const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; + struct mission_item_s missionitem = {}; + struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START + + _land_start_available = false; + + bool found_land_start_marker = false; + + for (size_t i = 1; i < _mission.count; i++) { + const ssize_t len = sizeof(missionitem); + missionitem_prev = missionitem; // store the last mission item before reading a new one + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + PX4_ERR("dataman read failure"); + break; + } + + if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { + found_land_start_marker = true; + _land_start_index = i; + } + + if (found_land_start_marker && !_land_start_available && i > _land_start_index + && item_contains_position(missionitem)) { + // use the position of any waypoint after the land start marker which specifies a position. + _landing_start_lat = missionitem.lat; + _landing_start_lon = missionitem.lon; + _landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude + + _navigator->get_home_position()->alt : missionitem.altitude; + _land_start_available = true; + } + + if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || + (missionitem.nav_cmd == NAV_CMD_LAND)) { + + _landing_lat = missionitem.lat; + _landing_lon = missionitem.lon; + _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt : + missionitem.altitude; + + // don't have a valid land start yet, use the landing item itself then + if (!_land_start_available) { + _land_start_index = i; + _landing_start_lat = _landing_lat; + _landing_start_lon = _landing_lon; + _landing_start_alt = _landing_alt; + _land_start_available = true; + } + + } + } + + return _land_start_available; +} + +bool +Mission::land_start() +{ + // if not currently landing, jump to do_land_start + if (_land_start_available) { + if (_navigator->getMissionLandingInProgress()) { + return true; + + } else { + set_current_mission_index(get_land_start_index()); + + const bool can_land_now = landing(); + _navigator->setMissionLandingInProgress(can_land_now); + return can_land_now; + } + } + + return false; +} + +bool +Mission::landing() +{ + // vehicle is currently landing if + // mission valid, still flying, and in the landing portion of mission + + const bool mission_valid = _navigator->get_mission_result()->valid; + const bool on_landing_stage = _land_start_available && (_current_mission_index >= get_land_start_index()); + + return mission_valid && on_landing_stage; +} + +void +Mission::update_mission() +{ + + bool failed = true; + + /* Reset vehicle_roi + * Missions that do not explicitly configure ROI would not override + * an existing ROI setting from previous missions */ + _navigator->reset_vroi(); + + const mission_s old_mission = _mission; + + if (_mission_sub.copy(&_mission)) { + /* determine current index */ + if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { + _current_mission_index = _mission.current_seq; + + } else { + /* if less items available, reset to first item */ + if (_current_mission_index >= (int)_mission.count) { + _current_mission_index = 0; + + } else if (_current_mission_index < 0) { + /* if not initialized, set it to 0 */ + _current_mission_index = 0; + } + + /* otherwise, just leave it */ + } + + check_mission_valid(true); + + failed = !_navigator->get_mission_result()->valid; + + if (!failed) { + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->failure = false; + + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->seq_total = _mission.count; + + /* reset work item if new mission has been accepted */ + _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _mission_changed = true; + } + + /* check if the mission waypoints changed while the vehicle is in air + * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ + if (((_mission.count != old_mission.count) || + (_mission.dataman_id != old_mission.dataman_id)) && + !_navigator->get_land_detected()->landed) { + _mission_waypoints_changed = true; + } + + } else { + PX4_ERR("mission update failed"); + } + + if (failed) { + // only warn if the check failed on merit + if ((int)_mission.count > 0) { + PX4_WARN("mission check failed"); + } + + // reset the mission + _mission.count = 0; + _mission.current_seq = 0; + _current_mission_index = 0; + } + + // find and store landing start marker (if available) + find_mission_land_start(); + + set_current_mission_item(); +} + + +void +Mission::advance_mission() +{ + /* do not advance mission item if we're processing sub mission work items */ + if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) { + return; + } + + switch (_mission_type) { + case MISSION_TYPE_MISSION: + switch (_mission_execution_mode) { + case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: + case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { + _current_mission_index++; + break; + } + + case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { + // find next position item in reverse order + dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); + + for (int32_t i = _current_mission_index - 1; i >= 0; i--) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(missionitem); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + PX4_ERR("dataman read failure"); + break; + } + + if (item_contains_position(missionitem)) { + _current_mission_index = i; + return; + } + } + + // finished flying back the mission + _current_mission_index = -1; + break; + } + + default: + _current_mission_index++; + } + + break; + + case MISSION_TYPE_NONE: + default: + break; + } +} + +void +Mission::set_mission_items() +{ + /* the home dist check provides user feedback, so we initialize it to this */ + bool user_feedback_done = false; + + /* mission item that comes after current if available */ + struct mission_item_s mission_item_next_position; + struct mission_item_s mission_item_after_next_position; + bool has_next_position_item = false; + bool has_after_next_position_item = false; + + work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + + if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item, + &mission_item_after_next_position, &has_after_next_position_item)) { + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_MISSION) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), + _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Executing Reverse Mission" : + "Executing Mission"); + user_feedback_done = true; + } + + _mission_type = MISSION_TYPE_MISSION; + + } else { + /* no mission available or mission finished, switch to loiter */ + if (_mission_type != MISSION_TYPE_NONE) { + + if (_navigator->get_land_detected()->landed) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), + _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed" : + "Mission finished, landed."); + + } else { + /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), + _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, loitering" : + "Mission finished, loitering."); + + /* use last setpoint for loiter */ + _navigator->set_can_loiter_at_sp(true); + } + + user_feedback_done = true; + } + + _mission_type = MISSION_TYPE_NONE; + + /* set loiter mission item and ensure that there is a minimum clearance from home */ + set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt()); + + /* update position setpoint triplet */ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + /* reuse setpoint for LOITER only if it's not IDLE */ + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + + // set mission finished + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + + if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + * https://en.wikipedia.org/wiki/Loiter_(aeronautics) + */ + + if (_navigator->get_land_detected()->landed) { + /* landed, refusing to take off without a mission */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff."); + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering."); + } + + user_feedback_done = true; + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + return; + } + + /*********************************** handle mission item *********************************************/ + + /* handle mission items depending on the mode */ + + const position_setpoint_s current_setpoint_copy = _navigator->get_position_setpoint_triplet()->current; + + if (item_contains_position(_mission_item)) { + switch (_mission_execution_mode) { + case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: + case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { + /* force vtol land */ + if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { + _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; + } + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* do takeoff before going to setpoint if needed and not already in takeoff */ + /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ + if (do_need_vertical_takeoff() && + _work_item_type == WORK_ITEM_TYPE_DEFAULT && + new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + + new_work_item_type = WORK_ITEM_TYPE_TAKEOFF; + + /* use current mission item as next position item */ + mission_item_next_position = _mission_item; + mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT; + has_next_position_item = true; + + float takeoff_alt = calculate_takeoff_altitude(&_mission_item); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home.", + (double)(takeoff_alt - _navigator->get_home_position()->alt)); + + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + /* hold heading for takeoff items */ + _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + && _work_item_type == WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == WORK_ITEM_TYPE_DEFAULT + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + + } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + /* haven't transitioned yet, trigger vtol takeoff logic below */ + _work_item_type = WORK_ITEM_TYPE_TAKEOFF; + + } else { + /* already in fixed-wing, go to waypoint */ + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + } + + /* if we just did a normal takeoff navigate to the actual waypoint now */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && + _work_item_type == WORK_ITEM_TYPE_TAKEOFF && + new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + } + + /* if we just did a VTOL takeoff, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WORK_ITEM_TYPE_TAKEOFF && + new_work_item_type == WORK_ITEM_TYPE_DEFAULT && + _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_navigator->get_land_detected()->landed) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.disable_weather_vane = true; + + /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + + _mission_item.force_heading = true; + + new_work_item_type = WORK_ITEM_TYPE_ALIGN; + + /* set position setpoint to current while aligning */ + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + } + + /* heading is aligned now, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WORK_ITEM_TYPE_ALIGN && + _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_navigator->get_land_detected()->landed) { + + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + + /* check if the vtol_takeoff waypoint is on top of us */ + if (do_need_move_to_takeoff()) { + new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF; + } + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + /* set position setpoint to target during the transition */ + generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw); + } + + /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF + && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + + new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + } + + /* move to land wp as fixed wing */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND + && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) + && new_work_item_type == WORK_ITEM_TYPE_DEFAULT + && !_navigator->get_land_detected()->landed) { + + new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + mission_item_next_position = _mission_item; + has_next_position_item = true; + + float altitude = _navigator->get_global_position()->alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + } + + /* transition to MC */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND + && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND + && new_work_item_type == WORK_ITEM_TYPE_DEFAULT + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && !_navigator->get_land_detected()->landed) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _navigator->get_global_position()->alt; + _mission_item.altitude_is_relative = false; + + new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + } + + /* move to landing waypoint before descent if necessary */ + if (do_need_move_to_land() && + (_work_item_type == WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) && + new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + + new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + mission_item_next_position = _mission_item; + has_next_position_item = true; + + /* + * Ignoring waypoint altitude: + * Set altitude to the same as we have now to prevent descending too fast into + * the ground. Actual landing will descend anyway until it touches down. + * XXX: We might want to change that at some point if it is clear to the user + * what the altitude means on this waypoint type. + */ + float altitude = _navigator->get_global_position()->alt; + + if (pos_sp_triplet->current.valid + && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + + } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) { + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); + + } + } + + /* we just moved to the landing waypoint, now descend */ + if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && + new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); + + } + + } + + /* ignore yaw for landing items */ + /* XXX: if specified heading for landing is desired we could add another step before the descent + * that aligns the vehicle first */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + _mission_item.yaw = NAN; + } + + + // for fast forward convert certain types to simple waypoint + // XXX: add other types which should be ignored in fast forward + if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD && + ((_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) || + (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT))) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + } + + break; + } + + case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { + if (item_contains_position(_mission_item)) { + // convert mission item to a simple waypoint + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "MissionReverse: Got a non-position mission item, ignoring it"); + } + + break; + } + } + + } else { + /* handle non-position mission items such as commands */ + switch (_mission_execution_mode) { + case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: + case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* turn towards next waypoint before MC to FW transition */ + if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + && _work_item_type == WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == WORK_ITEM_TYPE_DEFAULT + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_navigator->get_land_detected()->landed + && has_next_position_item) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.disable_weather_vane = true; + + new_work_item_type = WORK_ITEM_TYPE_ALIGN; + + set_align_mission_item(&_mission_item, &mission_item_next_position); + + /* set position setpoint to target during the transition */ + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); + } + + /* yaw is aligned now */ + if (_work_item_type == WORK_ITEM_TYPE_ALIGN && + new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + + new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + + /* set position setpoint to target during the transition */ + pos_sp_triplet->previous = pos_sp_triplet->current; + generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw); + } + + // ignore certain commands in mission fast forward + if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) && + (_mission_item.nav_cmd == NAV_CMD_DELAY)) { + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + } + + break; + } + + case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { + // nothing to do, all commands are ignored + break; + } + } + + if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) { + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; + } + } + + /*********************************** set setpoints and check next *********************************************/ + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // The logic in this section establishes the tracking between the current waypoint + // which we are approaching and the next waypoint, which will tell us in which direction + // we will change our trajectory right after reaching it. + + // Because actions, gates and jump labels can be interleaved with waypoints, + // we are searching around the current mission item in the list to find the closest + // gate and the closest waypoint. We then store them separately. + + // Check if the mission item is a gate + // along the current trajectory + if (item_contains_gate(_mission_item)) { + + // The mission item is a gate, let's check if the next item in the list provides + // a position to go towards. + + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 + if (has_next_position_item) { + // We have a position, convert it to the setpoint and update setpoint triplet + mission_apply_limitation(mission_item_next_position); + mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); + } + + // ELSE: The current position setpoint stays unchanged. + + } else { + // The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items) + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 + if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + // ELSE: The current position setpoint stays unchanged. + } + + // Only set the previous position item if the current one really changed + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 + if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) && + !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { + pos_sp_triplet->previous = current_setpoint_copy; + } + + /* issue command if ready (will do nothing for position mission items) */ + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND + || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + + _need_takeoff = true; + } + + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_MISSION) { + set_current_mission_item(); + } + + // If the mission item under evaluation contains a gate, + // then we need to check if we have a next position item so + // the controller can fly the correct line between the + // current and next setpoint + if (item_contains_gate(_mission_item)) { + if (has_after_next_position_item) { + /* got next mission item, update setpoint triplet */ + mission_apply_limitation(mission_item_next_position); + mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); + + } else { + pos_sp_triplet->next.valid = false; + } + + } else { + /* allow the vehicle to decelerate before reaching a wp with a hold time */ + const bool brake_for_hold = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && get_time_inside(_mission_item) > FLT_EPSILON; + + if (_mission_item.autocontinue && !brake_for_hold) { + /* try to process next mission item */ + if (has_next_position_item) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } + + } else { + /* vehicle will be paused on current waypoint, don't set next item */ + pos_sp_triplet->next.valid = false; + } + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +bool +Mission::do_need_vertical_takeoff() +{ + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + float takeoff_alt = calculate_takeoff_altitude(&_mission_item); + + if (_navigator->get_land_detected()->landed) { + /* force takeoff if landed (additional protection) */ + _need_takeoff = true; + + } else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_altitude_acceptance_radius()) { + /* if in-air and already above takeoff height, don't do takeoff */ + _need_takeoff = false; + + } else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_altitude_acceptance_radius() + && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { + /* if in-air but below takeoff height and we have a takeoff item */ + _need_takeoff = true; + } + + /* check if current mission item is one that requires takeoff before */ + if (_need_takeoff && ( + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { + + _need_takeoff = false; + return true; + } + } + + return false; +} + +bool +Mission::do_need_move_to_land() +{ + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { + + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + return d_current > _navigator->get_acceptance_radius(); + } + + return false; +} + +bool +Mission::do_need_move_to_takeoff() +{ + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { + + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + return d_current > _navigator->get_acceptance_radius(); + } + + return false; +} + +void +Mission::copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint) +{ + if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + mission_item->lat = setpoint->lat; + mission_item->lon = setpoint->lon; + mission_item->altitude = setpoint->alt; + + } else { + mission_item->lat = _navigator->get_global_position()->lat; + mission_item->lon = _navigator->get_global_position()->lon; + mission_item->altitude = _navigator->get_global_position()->alt; + } + + mission_item->altitude_is_relative = false; +} + +void +Mission::set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next) +{ + mission_item->nav_cmd = NAV_CMD_WAYPOINT; + copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); + mission_item->altitude_is_relative = false; + mission_item->autocontinue = true; + mission_item->time_inside = 0.0f; + mission_item->yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + mission_item_next->lat, mission_item_next->lon); + mission_item->force_heading = true; +} + +float +Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) +{ + /* calculate takeoff altitude */ + float takeoff_alt = get_absolute_altitude_for_item(*mission_item); + + /* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + if (_navigator->get_land_detected()->landed) { + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt()); + + } else { + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _navigator->get_takeoff_min_alt()); + } + + return takeoff_alt; +} + +void +Mission::heading_sp_update() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = + _navigator->get_position_setpoint_triplet(); + + // Only update if current triplet is valid + if (pos_sp_triplet->current.valid) { + + double point_from_latlon[2] = { _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon + }; + double point_to_latlon[2] = { _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon + }; + float yaw_offset = 0.0f; + + // Depending on ROI-mode, update heading + switch (_navigator->get_vroi().mode) { + case vehicle_roi_s::ROI_LOCATION: { + // ROI is a fixed location. Vehicle needs to point towards that location + point_to_latlon[0] = _navigator->get_vroi().lat; + point_to_latlon[1] = _navigator->get_vroi().lon; + // No yaw offset required + yaw_offset = 0.0f; + break; + } + + case vehicle_roi_s::ROI_WPNEXT: { + // ROI is current waypoint. Vehcile needs to point towards current waypoint + point_to_latlon[0] = pos_sp_triplet->current.lat; + point_to_latlon[1] = pos_sp_triplet->current.lon; + // Add the gimbal's yaw offset + yaw_offset = _navigator->get_vroi().yaw_offset; + break; + } + + case vehicle_roi_s::ROI_NONE: + case vehicle_roi_s::ROI_WPINDEX: + case vehicle_roi_s::ROI_TARGET: + case vehicle_roi_s::ROI_ENUM_END: + default: { + return; + } + } + + // Get desired heading and update it. + // However, only update if distance to desired heading is + // larger than acceptance radius to prevent excessive yawing + float d_current = get_distance_to_next_waypoint(point_from_latlon[0], + point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); + + if (d_current > _navigator->get_acceptance_radius()) { + float yaw = matrix::wrap_pi( + get_bearing_to_next_waypoint(point_from_latlon[0], + point_from_latlon[1], point_to_latlon[0], + point_to_latlon[1]) + yaw_offset); + + _mission_item.yaw = yaw; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + + } else { + if (!pos_sp_triplet->current.yaw_valid) { + _mission_item.yaw = _navigator->get_local_position()->heading; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + } + } + + // we set yaw directly so we can run this in parallel to the FOH update + publish_navigator_mission_item(); + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void +Mission::cruising_speed_sp_update() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + const float cruising_speed = _navigator->get_cruising_speed(); + + /* Don't change setpoint if the current waypoint is not valid */ + if (!pos_sp_triplet->current.valid || + fabsf(pos_sp_triplet->current.cruising_speed - cruising_speed) < FLT_EPSILON) { + return; + } + + pos_sp_triplet->current.cruising_speed = cruising_speed; + + publish_navigator_mission_item(); + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Mission::do_abort_landing() +{ + // Abort FW landing + // first climb out then loiter over intended landing location + + if (_mission_item.nav_cmd != NAV_CMD_LAND) { + return; + } + + // loiter at the larger of MIS_LTRMIN_ALT above the landing point + // or 2 * FW_CLMBOUT_DIFF above the current altitude + const float alt_landing = get_absolute_altitude_for_item(_mission_item); + const float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), + _navigator->get_global_position()->alt + 20.0f); + + // turn current landing waypoint into an indefinite loiter + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = alt_sp; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.", + (int)(alt_sp - alt_landing)); + + // reset mission index to start of landing + if (_land_start_available) { + _current_mission_index = get_land_start_index(); + + } else { + // move mission index back (landing approach point) + _current_mission_index -= 1; + } + + // send reposition cmd to get out of mission + vehicle_command_s vcmd = {}; + + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vcmd.param1 = -1; + vcmd.param2 = 1; + vcmd.param5 = _mission_item.lat; + vcmd.param6 = _mission_item.lon; + vcmd.param7 = alt_sp; + + _navigator->publish_vehicle_cmd(&vcmd); +} + +bool +Mission::prepare_mission_items(struct mission_item_s *mission_item, + struct mission_item_s *next_position_mission_item, bool *has_next_position_item, + struct mission_item_s *after_next_position_mission_item, bool *has_after_next_position_item) +{ + *has_next_position_item = false; + bool first_res = false; + int offset = 1; + + if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { + offset = -1; + } + + if (read_mission_item(0, mission_item)) { + + first_res = true; + + /* trying to find next position mission item */ + while (read_mission_item(offset, next_position_mission_item)) { + if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { + offset--; + + } else { + offset++; + } + + if (item_contains_position(*next_position_mission_item)) { + *has_next_position_item = true; + break; + } + } + + if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE && + after_next_position_mission_item && has_after_next_position_item) { + /* trying to find next next position mission item */ + while (read_mission_item(offset, after_next_position_mission_item)) { + offset++; + + if (item_contains_position(*after_next_position_mission_item)) { + *has_after_next_position_item = true; + break; + } + } + } + } + + return first_res; +} + +bool +Mission::read_mission_item(int offset, struct mission_item_s *mission_item) +{ + /* select mission */ + const int current_index = _current_mission_index; + int index_to_read = current_index + offset; + + int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read; + const dm_item_t dm_item = (dm_item_t)_mission.dataman_id; + + /* do not work on empty missions */ + if (_mission.count == 0) { + return false; + } + + /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after + * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ + for (int i = 0; i < 10; i++) { + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)_mission.count) { + /* mission item index out of bounds - if they are equal, we just reached the end */ + if ((*mission_index_ptr != (int)_mission.count) && (*mission_index_ptr != -1)) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item index out of bound, index: %d, max: %" PRIu16 ".", + *mission_index_ptr, _mission.count); + } + + return false; + } + + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item to temp storage first to not overwrite current mission item if data damaged */ + struct mission_item_s mission_item_tmp; + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read."); + return false; + } + + /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ + if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { + const bool execute_jumps = _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL; + + /* do DO_JUMP as many times as requested if not in reverse mode */ + if ((mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) && execute_jumps) { + + /* only raise the repeat count if this is for the current mission item + * but not for the read ahead mission item */ + if (offset == 0) { + (mission_item_tmp.do_jump_current_count)++; + + /* save repeat count */ + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, &mission_item_tmp, len) != len) { + /* not supposed to happen unless the datamanager can't access the dataman */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written."); + return false; + } + + report_do_jump_mission_changed(*mission_index_ptr, mission_item_tmp.do_jump_repeat_count); + } + + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index_ptr = mission_item_tmp.do_jump_mission_index; + + } else { + if (offset == 0 && execute_jumps) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed."); + } + + /* no more DO_JUMPS, therefore just try to continue with next mission item */ + if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { + (*mission_index_ptr)--; + + } else { + (*mission_index_ptr)++; + } + } + + } else { + /* if it's not a DO_JUMP, then we were successful */ + memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); + return true; + } + } + + /* we have given up, we don't want to cycle forever */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up."); + return false; +} + +void +Mission::save_mission_state() +{ + mission_s mission_state = {}; + + /* lock MISSION_STATE item */ + int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + + if (dm_lock_ret != 0) { + PX4_ERR("DM_KEY_MISSION_STATE lock failed"); + } + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + if (read_res == sizeof(mission_s)) { + /* data read successfully, check dataman ID and items count */ + if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) { + /* navigator may modify only sequence, write modified state only if it changed */ + if (mission_state.current_seq != _current_mission_index) { + mission_state.current_seq = _current_mission_index; + mission_state.timestamp = hrt_absolute_time(); + + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, + sizeof(mission_s)) != sizeof(mission_s)) { + + PX4_ERR("Can't save mission state"); + } + } + } + + } else { + /* invalid data, this must not happen and indicates error in mission publisher */ + mission_state.timestamp = hrt_absolute_time(); + mission_state.dataman_id = _mission.dataman_id; + mission_state.count = _mission.count; + mission_state.current_seq = _current_mission_index; + + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Invalid mission state."); + + /* write modified state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, + sizeof(mission_s)) != sizeof(mission_s)) { + + PX4_ERR("Can't save mission state"); + } + } + + /* unlock MISSION_STATE item */ + if (dm_lock_ret == 0) { + dm_unlock(DM_KEY_MISSION_STATE); + } +} + +void +Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + + _navigator->set_mission_result_updated(); +} + +void +Mission::set_mission_item_reached() +{ + _navigator->get_mission_result()->seq_reached = _current_mission_index; + _navigator->set_mission_result_updated(); + + // let the navigator know that we are currently executing the mission landing. + // Using the method landing() itself is not accurate as it only give information about the mission index + // but the vehicle could still be very far from the actual landing items + _navigator->setMissionLandingInProgress(landing()); + + reset_mission_item_reached(); +} + +void +Mission::set_current_mission_item() +{ + _navigator->get_mission_result()->finished = false; + _navigator->get_mission_result()->seq_current = _current_mission_index; + + _navigator->set_mission_result_updated(); + + save_mission_state(); +} + +void +Mission::check_mission_valid(bool force) +{ + if ((!_home_inited && _navigator->home_position_valid()) || force) { + + MissionFeasibilityChecker _missionFeasibilityChecker(_navigator); + + _navigator->get_mission_result()->valid = + _missionFeasibilityChecker.checkMissionFeasible(_mission, + _param_mis_dist_1wp.get(), + _param_mis_dist_wps.get(), + _navigator->mission_landing_required()); + + _navigator->get_mission_result()->seq_total = _mission.count; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + _home_inited = _navigator->home_position_valid(); + + // find and store landing start marker (if available) + find_mission_land_start(); + } +} + +void +Mission::reset_mission(struct mission_s &mission) +{ + dm_lock(DM_KEY_MISSION_STATE); + + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { + /* set current item to 0 */ + mission.current_seq = 0; + + /* reset jump counters */ + if (mission.count > 0) { + const dm_item_t dm_current = (dm_item_t)mission.dataman_id; + + for (unsigned index = 0; index < mission.count; index++) { + struct mission_item_s item; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, index, &item, len) != len) { + PX4_WARN("could not read mission item during reset"); + break; + } + + if (item.nav_cmd == NAV_CMD_DO_JUMP) { + item.do_jump_current_count = 0; + + if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET, &item, len) != len) { + PX4_WARN("could not save mission item during reset"); + break; + } + } + } + } + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission."); + + /* initialize mission state in dataman */ + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.count = 0; + mission.current_seq = 0; + } + + dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + } + + dm_unlock(DM_KEY_MISSION_STATE); +} + +bool +Mission::need_to_reset_mission() +{ + /* reset mission state when disarmed */ + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) { + _need_mission_reset = false; + return true; + } + + return false; +} + +void +Mission::generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw) +{ + waypoint_from_heading_and_distance( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + yaw, 1000000.0f, + &(setpoint->lat), &(setpoint->lon)); + setpoint->type = position_setpoint_s::SETPOINT_TYPE_POSITION; + setpoint->yaw = yaw; +} + +int32_t +Mission::index_closest_mission_item() const +{ + int32_t min_dist_index(0); + float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); + + dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); + + for (size_t i = 0; i < _mission.count; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(missionitem); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + PX4_ERR("dataman read failure"); + break; + } + + if (item_contains_position(missionitem)) { + // do not consider land waypoints for a fw + if (!((missionitem.nav_cmd == NAV_CMD_LAND) && + (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + (!_navigator->get_vstatus()->is_vtol))) { + float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon, + get_absolute_altitude_for_item(missionitem), + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + if (dist < min_dist) { + min_dist = dist; + min_dist_index = i; + } + } + } + } + + // for mission reverse also consider the home position + if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { + float dist = get_distance_to_point_global_wgs84( + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon, + _navigator->get_home_position()->alt, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + if (dist < min_dist) { + min_dist = dist; + min_dist_index = -1; + } + } + + return min_dist_index; +} + +bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const +{ + return ((p1->valid == p2->valid) && + (p1->type == p2->type) && + (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && + (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && + (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && + (p1->velocity_valid == p2->velocity_valid) && + (p1->velocity_frame == p2->velocity_frame) && + (p1->alt_valid == p2->alt_valid) && + (fabs(p1->lat - p2->lat) < DBL_EPSILON) && + (fabs(p1->lon - p2->lon) < DBL_EPSILON) && + (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && + ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && + (p1->yaw_valid == p2->yaw_valid) && + (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && + (p1->yawspeed_valid == p2->yawspeed_valid) && + (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && + (p1->loiter_direction == p2->loiter_direction) && + (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && + (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && + ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) + && !PX4_ISFINITE(p2->cruising_throttle)))); + +} + +void Mission::publish_navigator_mission_item() +{ + navigator_mission_item_s navigator_mission_item{}; + + navigator_mission_item.instance_count = _navigator->mission_instance_count(); + navigator_mission_item.sequence_current = _current_mission_index; + navigator_mission_item.nav_cmd = _mission_item.nav_cmd; + navigator_mission_item.latitude = _mission_item.lat; + navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.altitude = _mission_item.altitude; + + navigator_mission_item.time_inside = get_time_inside(_mission_item); + navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius; + navigator_mission_item.loiter_radius = _mission_item.loiter_radius; + navigator_mission_item.yaw = _mission_item.yaw; + + navigator_mission_item.frame = _mission_item.frame; + navigator_mission_item.frame = _mission_item.origin; + + navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack; + navigator_mission_item.force_heading = _mission_item.force_heading; + navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative; + navigator_mission_item.autocontinue = _mission_item.autocontinue; + navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; + + navigator_mission_item.timestamp = hrt_absolute_time(); + + _navigator_mission_item_pub.publish(navigator_mission_item); +} diff --git a/src/prometheus_px4/src/modules/navigator/mission.h b/src/prometheus_px4/src/modules/navigator/mission.h new file mode 100644 index 0000000..308b238 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/mission.h @@ -0,0 +1,291 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ +/** + * @file mission.h + * + * Navigator mode to access missions + * + * @author Julian Oes + * @author Thomas Gubler + * @author Anton Babushkin + * @author Ban Siesta + * @author Lorenz Meier + */ + +#pragma once + +#include "mission_block.h" +#include "mission_feasibility_checker.h" +#include "navigator_mode.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Navigator; + +class Mission : public MissionBlock, public ModuleParams +{ +public: + Mission(Navigator *navigator); + ~Mission() override = default; + + void on_inactive() override; + void on_inactivation() override; + void on_activation() override; + void on_active() override; + + bool set_current_mission_index(uint16_t index); + + bool land_start(); + bool landing(); + + uint16_t get_land_start_index() const { return _land_start_index; } + bool get_land_start_available() const { return _land_start_available; } + bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; } + bool get_mission_changed() const { return _mission_changed ; } + bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; } + double get_landing_start_lat() { return _landing_start_lat; } + double get_landing_start_lon() { return _landing_start_lon; } + float get_landing_start_alt() { return _landing_start_alt; } + + double get_landing_lat() { return _landing_lat; } + double get_landing_lon() { return _landing_lon; } + float get_landing_alt() { return _landing_alt; } + + void set_closest_item_as_current(); + + /** + * Set a new mission mode and handle the switching between the different modes + * + * For a list of the different modes refer to mission_result.msg + */ + void set_execution_mode(const uint8_t mode); +private: + + /** + * Update mission topic + */ + void update_mission(); + + /** + * Move on to next mission item or switch to loiter + */ + void advance_mission(); + + /** + * Set new mission items + */ + void set_mission_items(); + + /** + * Returns true if we need to do a takeoff at the current state + */ + bool do_need_vertical_takeoff(); + + /** + * Returns true if we need to move to waypoint location before starting descent + */ + bool do_need_move_to_land(); + + /** + * Returns true if we need to move to waypoint location after vtol takeoff + */ + bool do_need_move_to_takeoff(); + + /** + * Copies position from setpoint if valid, otherwise copies current position + */ + void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint); + + /** + * Create mission item to align towards next waypoint + */ + void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next); + + /** + * Calculate takeoff height for mission item considering ground clearance + */ + float calculate_takeoff_altitude(struct mission_item_s *mission_item); + + /** + * Updates the heading of the vehicle. Rotary wings only. + */ + void heading_sp_update(); + + /** + * Update the cruising speed setpoint. + */ + void cruising_speed_sp_update(); + + /** + * Abort landing + */ + void do_abort_landing(); + + /** + * Read the current and the next mission item. The next mission item read is the + * next mission item that contains a position. + * + * @return true if current mission item available + */ + bool prepare_mission_items(mission_item_s *mission_item, + mission_item_s *next_position_mission_item, bool *has_next_position_item, + mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr); + + /** + * Read current (offset == 0) or a specific (offset > 0) mission item + * from the dataman and watch out for DO_JUMPS + * + * @return true if successful + */ + bool read_mission_item(int offset, struct mission_item_s *mission_item); + + /** + * Save current mission state to dataman + */ + void save_mission_state(); + + /** + * Inform about a changed mission item after a DO_JUMP + */ + void report_do_jump_mission_changed(int index, int do_jumps_remaining); + + /** + * Set a mission item as reached + */ + void set_mission_item_reached(); + + /** + * Set the current mission item + */ + void set_current_mission_item(); + + /** + * Check whether a mission is ready to go + */ + void check_mission_valid(bool force); + + /** + * Reset mission + */ + void reset_mission(struct mission_s &mission); + + /** + * Returns true if we need to reset the mission (call this only when inactive) + */ + bool need_to_reset_mission(); + + /** + * Project current location with heading to far away location and fill setpoint. + */ + void generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw); + + /** + * Find and store the index of the landing sequence (DO_LAND_START) + */ + bool find_mission_land_start(); + + /** + * Return the index of the closest mission item to the current global position. + */ + int32_t index_closest_mission_item() const; + + bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; + + void publish_navigator_mission_item(); + + DEFINE_PARAMETERS( + (ParamFloat) _param_mis_dist_1wp, + (ParamFloat) _param_mis_dist_wps, + (ParamInt) _param_mis_mnt_yaw_ctl + ) + + uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; + + uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ + mission_s _mission {}; + + int32_t _current_mission_index{-1}; + + // track location of planned mission landing + bool _land_start_available{false}; + uint16_t _land_start_index{UINT16_MAX}; /**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */ + double _landing_start_lat{0.0}; + double _landing_start_lon{0.0}; + float _landing_start_alt{0.0f}; + + double _landing_lat{0.0}; + double _landing_lon{0.0}; + float _landing_alt{0.0f}; + + bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + + hrt_abstime _time_mission_deactivated{0}; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_MISSION + } _mission_type{MISSION_TYPE_NONE}; + + bool _inited{false}; + bool _home_inited{false}; + bool _need_mission_reset{false}; + bool _mission_waypoints_changed{false}; + bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */ + + enum work_item_type { + WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ + WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ + WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ + WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ + WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, + WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, + WORK_ITEM_TYPE_PRECISION_LAND + } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ + + uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */ + bool _execution_mode_changed{false}; +}; diff --git a/src/prometheus_px4/src/modules/navigator/mission_block.cpp b/src/prometheus_px4/src/modules/navigator/mission_block.cpp new file mode 100644 index 0000000..7c76899 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/mission_block.cpp @@ -0,0 +1,795 @@ +/**************************************************************************** + * + * Copyright (c) 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 mission_block.cpp + * + * Helper class to use mission items + * + * @author Julian Oes + * @author Sander Smeets + * @author Andreas Antener + */ + +#include "mission_block.h" +#include "navigator.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using matrix::wrap_pi; + +MissionBlock::MissionBlock(Navigator *navigator) : + NavigatorMode(navigator) +{ + _mission_item.lat = (double)NAN; + _mission_item.lon = (double)NAN; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; +} + +bool +MissionBlock::is_mission_item_reached() +{ + /* handle non-navigation or indefinite waypoints */ + + switch (_mission_item.nav_cmd) { + case NAV_CMD_DO_SET_SERVO: + return true; + + case NAV_CMD_LAND: /* fall through */ + case NAV_CMD_VTOL_LAND: + return _navigator->get_land_detected()->landed; + + case NAV_CMD_IDLE: /* fall through */ + case NAV_CMD_LOITER_UNLIMITED: + return false; + + case NAV_CMD_DO_LAND_START: + case NAV_CMD_DO_TRIGGER_CONTROL: + case NAV_CMD_DO_DIGICAM_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + case NAV_CMD_VIDEO_START_CAPTURE: + case NAV_CMD_VIDEO_STOP_CAPTURE: + case NAV_CMD_DO_CONTROL_VIDEO: + case NAV_CMD_DO_MOUNT_CONFIGURE: + case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + case NAV_CMD_DO_SET_ROI: + case NAV_CMD_DO_SET_ROI_LOCATION: + case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: + case NAV_CMD_DO_SET_ROI_NONE: + case NAV_CMD_DO_SET_CAM_TRIGG_DIST: + case NAV_CMD_OBLIQUE_SURVEY: + case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: + case NAV_CMD_SET_CAMERA_MODE: + case NAV_CMD_SET_CAMERA_ZOOM: + case NAV_CMD_SET_CAMERA_FOCUS: + return true; + + case NAV_CMD_DO_VTOL_TRANSITION: + + if (int(_mission_item.params[0]) == 3) { + // transition to RW requested, only accept waypoint if vehicle state has changed accordingly + return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + } else if (int(_mission_item.params[0]) == 4) { + // transition to FW requested, only accept waypoint if vehicle state has changed accordingly + return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + } else { + // invalid vtol transition request + return false; + } + + case NAV_CMD_DO_CHANGE_SPEED: + case NAV_CMD_DO_SET_HOME: + return true; + + default: + /* do nothing, this is a 3D waypoint */ + break; + } + + hrt_abstime now = hrt_absolute_time(); + + if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) { + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item); + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + /* We want to avoid the edge case where the acceptance radius is bigger or equal than + * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow + * take-off procedures like leaving the landing gear down. */ + + float takeoff_alt = _mission_item.altitude_is_relative ? + _mission_item.altitude : + (_mission_item.altitude - _navigator->get_home_position()->alt); + + float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius(); + + /* It should be safe to just use half of the takoeff_alt as an acceptance radius. */ + if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) { + altitude_acceptance_radius = takeoff_alt / 2.0f; + } + + /* require only altitude for takeoff for multicopter */ + if (_navigator->get_global_position()->alt > + mission_item_altitude_amsl - altitude_acceptance_radius) { + _waypoint_position_reached = true; + } + + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + /* for takeoff mission items use the parameter for the takeoff acceptance radius */ + if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) { + _waypoint_position_reached = true; + } + + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + /* for takeoff mission items use the parameter for the takeoff acceptance radius */ + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius() + && dist_z <= _navigator->get_altitude_acceptance_radius()) { + _waypoint_position_reached = true; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && + (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { + + /* Loiter mission item on a non rotary wing: the aircraft is going to circle the + * coordinates with a radius equal to the loiter_radius field. It is not flying + * through the waypoint center. + * Therefore the item is marked as reached once the system reaches the loiter + * radius + L1 distance. Time inside and turn count is handled elsewhere. + */ + + // check if within loiter radius around wp, if yes then set altitude sp to mission item + if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) + && dist_z <= _navigator->get_altitude_acceptance_radius()) { + + _waypoint_position_reached = true; + } + + } else if (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter. + // First check if the altitude setpoint is the mission setpoint (that means that the loiter is not yet reached) + struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + + if (fabsf(curr_sp->alt - mission_item_altitude_amsl) >= FLT_EPSILON) { + dist_xy = -1.0f; + dist_z = -1.0f; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + // check if within loiter radius around wp, if yes then set altitude sp to mission item + if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) + && dist_z <= _navigator->get_default_altitude_acceptance_radius()) { + + curr_sp->alt = mission_item_altitude_amsl; + curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; + _navigator->set_position_setpoint_triplet_updated(); + } + + } else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) + && dist_z <= _navigator->get_altitude_acceptance_radius()) { + // loitering, check if new altitude is reached, while still also having check on position + + _waypoint_position_reached = true; + } + + } else if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) { + + struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + + // if the setpoint is valid we are checking if we reached the gate + // in the case of an invalid setpoint we are defaulting to + // assuming that we have already reached the gate to not block + // the further execution of the mission. + if (curr_sp->valid) { + + // location of gate (mission item) + struct map_projection_reference_s ref_pos; + map_projection_init(&ref_pos, _mission_item.lat, _mission_item.lon); + + // current setpoint + matrix::Vector2f gate_to_curr_sp; + map_projection_project(&ref_pos, curr_sp->lat, curr_sp->lon, &gate_to_curr_sp(0), &gate_to_curr_sp(1)); + + // system position + matrix::Vector2f vehicle_pos; + map_projection_project(&ref_pos, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + &vehicle_pos(0), &vehicle_pos(1)); + const float dot_product = vehicle_pos.dot(gate_to_curr_sp.normalized()); + + // if the dot product (projected vector) is positive, then + // the current position is between the gate position and the + // next waypoint + if (dot_product >= 0) { + _waypoint_position_reached = true; + _waypoint_yaw_reached = true; + _time_wp_reached = now; + } + } + + } else if (_mission_item.nav_cmd == NAV_CMD_DELAY) { + _waypoint_position_reached = true; + _waypoint_yaw_reached = true; + _time_wp_reached = now; + + } else { + + float acceptance_radius = _navigator->get_acceptance_radius(); + + // We use the acceptance radius of the mission item if it has been set (not NAN) + // but only for multicopter. + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && PX4_ISFINITE(_mission_item.acceptance_radius) && _mission_item.acceptance_radius > FLT_EPSILON) { + acceptance_radius = _mission_item.acceptance_radius; + } + + /* for vtol back transition calculate acceptance radius based on time and ground speed */ + if (_mission_item.vtol_back_transition + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx + + _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); + + const float back_trans_dec = _navigator->get_vtol_back_trans_deceleration(); + const float reverse_delay = _navigator->get_vtol_reverse_delay(); + + if (back_trans_dec > FLT_EPSILON && velocity > FLT_EPSILON) { + acceptance_radius = ((velocity / back_trans_dec / 2) * velocity) + reverse_delay * velocity; + + } + + } + + if (dist_xy >= 0.0f && dist_xy <= acceptance_radius + && dist_z <= _navigator->get_altitude_acceptance_radius()) { + _waypoint_position_reached = true; + } + } + + if (_waypoint_position_reached) { + // reached just now + _time_wp_reached = now; + } + + // consider yaw reached for non-rotary wing vehicles (such as fixed-wing) + if (_navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _waypoint_yaw_reached = true; + } + } + + /* Check if the requested yaw setpoint is reached (only for rotary wing flight). */ + + if (_waypoint_position_reached && !_waypoint_yaw_reached) { + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) { + + const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading); + + /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ + if (fabsf(yaw_err) < _navigator->get_yaw_threshold() + || (_navigator->get_yaw_timeout() >= FLT_EPSILON && !_mission_item.force_heading)) { + + _waypoint_yaw_reached = true; + } + + /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ + if (!_waypoint_yaw_reached && _mission_item.force_heading && + (_navigator->get_yaw_timeout() >= FLT_EPSILON) && + (now - _time_wp_reached >= (hrt_abstime)_navigator->get_yaw_timeout() * 1e6f)) { + + _navigator->set_mission_failure("unable to reach heading within timeout"); + } + + } else { + _waypoint_yaw_reached = true; + } + } + + /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { + + bool time_inside_reached = false; + + /* check if the MAV was long enough inside the waypoint orbit */ + if ((get_time_inside(_mission_item) < FLT_EPSILON) || + (now - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { + time_inside_reached = true; + } + + // check if heading for exit is reached (only applies for fixed-wing flight) + bool exit_heading_reached = false; + + if (time_inside_reached) { + + struct position_setpoint_s *curr_sp_new = &_navigator->get_position_setpoint_triplet()->current; + const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; + + /* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set, + or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */ + const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && + next_sp.valid && + curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER && + (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT); + + if (enforce_exit_heading) { + + + const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat, + next_sp.lon); + + float yaw_err = 0.0f; + + if (dist_current_next > 1.2f * _navigator->get_loiter_radius()) { + // set required yaw from bearing to the next mission item + _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + next_sp.lat, next_sp.lon); + const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx); + yaw_err = wrap_pi(_mission_item.yaw - cog); + + + + } + + + if (fabsf(yaw_err) < _navigator->get_yaw_threshold()) { + exit_heading_reached = true; + } + + } else { + exit_heading_reached = true; + } + } + + // set exit flight course to next waypoint + if (exit_heading_reached) { + position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current; + const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; + + const float range = get_distance_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); + + // exit xtrack location + // reset lat/lon of loiter waypoint so vehicle follows a tangent + if (_mission_item.loiter_exit_xtrack && next_sp.valid && PX4_ISFINITE(range) && + (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { + + float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); + // We should not use asinf outside of [-1..1]. + const float ratio = math::constrain(_mission_item.loiter_radius / range, -1.0f, 1.0f); + float inner_angle = M_PI_2_F - asinf(ratio); + + // Compute "ideal" tangent origin + if (curr_sp.loiter_direction > 0) { + bearing -= inner_angle; + + } else { + bearing += inner_angle; + } + + // Replace current setpoint lat/lon with tangent coordinate + waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon, + bearing, fabsf(curr_sp.loiter_radius), + &curr_sp.lat, &curr_sp.lon); + } + + return true; // mission item is reached + } + } + + return false; +} + +void +MissionBlock::reset_mission_item_reached() +{ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_wp_reached = 0; +} + +void +MissionBlock::issue_command(const mission_item_s &item) +{ + if (item_contains_position(item) + || item_contains_gate(item) + || item_contains_marker(item)) { + return; + } + + if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) { + PX4_INFO("DO_SET_SERVO command"); + + // XXX: we should issue a vehicle command and handle this somewhere else + actuator_controls_s actuators = {}; + actuators.timestamp = hrt_absolute_time(); + + // params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6) + // params[1] new value for selected actuator in ms 900...2000 + actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1]; + + _actuator_pub.publish(actuators); + + } else { + + // This is to support legacy DO_MOUNT_CONTROL as part of a mission. + if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { + _navigator->acquire_gimbal_control(); + } + + // we're expecting a mission command item here so assign the "raw" inputs to the command + // (MAV_FRAME_MISSION mission item) + vehicle_command_s vcmd = {}; + vcmd.command = item.nav_cmd; + vcmd.param1 = item.params[0]; + vcmd.param2 = item.params[1]; + vcmd.param3 = item.params[2]; + vcmd.param4 = item.params[3]; + + if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION && item.altitude_is_relative) { + vcmd.param5 = item.lat; + vcmd.param6 = item.lon; + vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; + + } else { + vcmd.param5 = (double)item.params[4]; + vcmd.param6 = (double)item.params[5]; + vcmd.param7 = item.params[6]; + } + + _navigator->publish_vehicle_cmd(&vcmd); + } +} + +float +MissionBlock::get_time_inside(const mission_item_s &item) const +{ + if ((item.nav_cmd == NAV_CMD_WAYPOINT + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) || + item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item.nav_cmd == NAV_CMD_DELAY) { + + // a negative time inside would be invalid + return math::max(item.time_inside, 0.0f); + } + + return 0.0f; +} + +bool +MissionBlock::item_contains_position(const mission_item_s &item) +{ + return item.nav_cmd == NAV_CMD_WAYPOINT || + item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item.nav_cmd == NAV_CMD_LAND || + item.nav_cmd == NAV_CMD_TAKEOFF || + item.nav_cmd == NAV_CMD_LOITER_TO_ALT || + item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || + item.nav_cmd == NAV_CMD_VTOL_LAND || + item.nav_cmd == NAV_CMD_DO_FOLLOW_REPOSITION; +} + +bool +MissionBlock::item_contains_gate(const mission_item_s &item) +{ + return item.nav_cmd == NAV_CMD_CONDITION_GATE; +} + +bool +MissionBlock::item_contains_marker(const mission_item_s &item) +{ + return item.nav_cmd == NAV_CMD_DO_LAND_START; +} + +bool +MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp) +{ + /* don't change the setpoint for non-position items */ + if (!item_contains_position(item)) { + return false; + } + + sp->lat = item.lat; + sp->lon = item.lon; + sp->alt = get_absolute_altitude_for_item(item); + sp->yaw = item.yaw; + sp->yaw_valid = PX4_ISFINITE(item.yaw); + sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : + _navigator->get_loiter_radius(); + sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1; + + if (item.acceptance_radius > 0.0f && PX4_ISFINITE(item.acceptance_radius)) { + // if the mission item has a specified acceptance radius, overwrite the default one from parameters + sp->acceptance_radius = item.acceptance_radius; + + } else { + sp->acceptance_radius = _navigator->get_default_acceptance_radius(); + } + + sp->cruising_speed = _navigator->get_cruising_speed(); + sp->cruising_throttle = _navigator->get_cruising_throttle(); + + switch (item.nav_cmd) { + case NAV_CMD_IDLE: + sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; + break; + + case NAV_CMD_TAKEOFF: + + // if already flying (armed and !landed) treat TAKEOFF like regular POSITION + if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) + && !_navigator->get_land_detected()->landed && !_navigator->get_land_detected()->maybe_landed) { + + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + } else { + sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + } + + break; + + case NAV_CMD_VTOL_TAKEOFF: + sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + break; + + case NAV_CMD_LAND: + case NAV_CMD_VTOL_LAND: + sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; + break; + + case NAV_CMD_LOITER_TO_ALT: + + // initially use current altitude, and switch to mission item altitude once in loiter position + if (_navigator->get_loiter_min_alt() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0 + sp->alt = math::max(_navigator->get_global_position()->alt, + _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt()); + + } else { + sp->alt = _navigator->get_global_position()->alt; + } + + // FALLTHROUGH + case NAV_CMD_LOITER_TIME_LIMIT: + case NAV_CMD_LOITER_UNLIMITED: + + sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; + break; + + default: + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; + break; + } + + sp->valid = true; + sp->timestamp = hrt_absolute_time(); + + return sp->valid; +} + +void +MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) +{ + if (_navigator->get_land_detected()->landed) { + /* landed, don't takeoff, but switch to IDLE mode */ + item->nav_cmd = NAV_CMD_IDLE; + + } else { + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { + /* use current position setpoint */ + item->lat = pos_sp_triplet->current.lat; + item->lon = pos_sp_triplet->current.lon; + item->altitude = pos_sp_triplet->current.alt; + + } else { + /* use current position and use return altitude as clearance */ + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude = _navigator->get_global_position()->alt; + + if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { + item->altitude = _navigator->get_home_position()->alt + min_clearance; + } + } + + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; + } +} + +void +MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) +{ + item->nav_cmd = NAV_CMD_TAKEOFF; + + /* use current position */ + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->yaw = _navigator->get_local_position()->heading; + + item->altitude = abs_altitude; + item->altitude_is_relative = false; + + item->loiter_radius = _navigator->get_loiter_radius(); + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; +} + +void +MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location) +{ + /* VTOL transition to RW before landing */ + if (_navigator->force_vtol()) { + + vehicle_command_s vcmd = {}; + vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; + vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + vcmd.param2 = 0.0f; + _navigator->publish_vehicle_cmd(&vcmd); + } + + /* set the land item */ + item->nav_cmd = NAV_CMD_LAND; + + /* use current position */ + if (at_current_location) { + item->lat = (double)NAN; //descend at current position + item->lon = (double)NAN; //descend at current position + item->yaw = _navigator->get_local_position()->heading; + + } else { + /* use home position */ + item->lat = _navigator->get_home_position()->lat; + item->lon = _navigator->get_home_position()->lon; + item->yaw = _navigator->get_home_position()->yaw; + } + + item->altitude = 0; + item->altitude_is_relative = false; + item->loiter_radius = _navigator->get_loiter_radius(); + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + +void +MissionBlock::set_idle_item(struct mission_item_s *item) +{ + item->nav_cmd = NAV_CMD_IDLE; + item->lat = _navigator->get_home_position()->lat; + item->lon = _navigator->get_home_position()->lon; + item->altitude_is_relative = false; + item->altitude = _navigator->get_home_position()->alt; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + +void +MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode) +{ + item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; + item->params[0] = (float) new_mode; + item->params[1] = 0.0f; + item->yaw = _navigator->get_local_position()->heading; + item->autocontinue = true; +} + +void +MissionBlock::mission_apply_limitation(mission_item_s &item) +{ + /* + * Limit altitude + */ + + /* do nothing if altitude max is negative */ + if (_navigator->get_land_detected()->alt_max > 0.0f) { + + /* absolute altitude */ + float altitude_abs = item.altitude_is_relative + ? item.altitude + _navigator->get_home_position()->alt + : item.altitude; + + /* limit altitude to maximum allowed altitude */ + if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) { + item.altitude = item.altitude_is_relative ? + _navigator->get_land_detected()->alt_max : + _navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt; + + } + } + + /* + * Add other limitations here + */ +} + +float +MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const +{ + if (mission_item.altitude_is_relative) { + return mission_item.altitude + _navigator->get_home_position()->alt; + + } else { + return mission_item.altitude; + } +} diff --git a/src/prometheus_px4/src/modules/navigator/mission_block.h b/src/prometheus_px4/src/modules/navigator/mission_block.h new file mode 100644 index 0000000..2de76f1 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/mission_block.h @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (c) 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 mission_block.h + * + * Helper class to use mission items + * + * @author Julian Oes + */ + +#pragma once + +#include "navigator_mode.h" +#include "navigation.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Navigator; + +class MissionBlock : public NavigatorMode +{ +public: + /** + * Constructor + */ + MissionBlock(Navigator *navigator); + virtual ~MissionBlock() = default; + + MissionBlock(const MissionBlock &) = delete; + MissionBlock &operator=(const MissionBlock &) = delete; + + /** + * Check if the mission item contains a navigation position + * + * @return false if the mission item does not contain a valid position + */ + static bool item_contains_position(const mission_item_s &item); + + /** + * Check if the mission item contains a gate condition + * + * @return true if mission item is a gate + */ + static bool item_contains_gate(const mission_item_s &item); + + /** + * Check if the mission item contains a marker + * + * @return true if mission item is a marker + */ + static bool item_contains_marker(const mission_item_s &item); + +protected: + /** + * Check if mission item has been reached + * @return true if successfully reached + */ + bool is_mission_item_reached(); + + /** + * Reset all reached flags + */ + void reset_mission_item_reached(); + + /** + * Convert a mission item to a position setpoint + * + * @param the mission item to convert + * @param the position setpoint that needs to be set + */ + bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp); + + /** + * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position + */ + void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f); + + /** + * Set a takeoff mission item + */ + void set_takeoff_item(struct mission_item_s *item, float abs_altitude); + + /** + * Set a land mission item + */ + void set_land_item(struct mission_item_s *item, bool at_current_location); + + /** + * Set idle mission item + */ + void set_idle_item(struct mission_item_s *item); + + /** + * Set vtol transition item + */ + void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode); + + /** + * General function used to adjust the mission item based on vehicle specific limitations + */ + void mission_apply_limitation(mission_item_s &item); + + void issue_command(const mission_item_s &item); + + float get_time_inside(const mission_item_s &item) const ; + + float get_absolute_altitude_for_item(const mission_item_s &mission_item) const; + + mission_item_s _mission_item{}; + + bool _waypoint_position_reached{false}; + bool _waypoint_yaw_reached{false}; + + hrt_abstime _time_wp_reached{0}; + + uORB::Publication _actuator_pub{ORB_ID(actuator_controls_2)}; +}; diff --git a/src/prometheus_px4/src/modules/navigator/mission_feasibility_checker.cpp b/src/prometheus_px4/src/modules/navigator/mission_feasibility_checker.cpp new file mode 100644 index 0000000..053ea98 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/mission_feasibility_checker.cpp @@ -0,0 +1,737 @@ +/**************************************************************************** + * + * 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 mission_feasibility_checker.cpp + * Provides checks if mission is feasible given the navigation capabilities + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Sander Smeets + * @author Nuno Marques + */ + +#include "mission_feasibility_checker.h" + +#include "mission_block.h" +#include "navigator.h" + +#include +#include +#include +#include +#include +#include +#include + +bool +MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, + float max_distance_to_1st_waypoint, float max_distance_between_waypoints, + bool land_start_req) +{ + // Reset warning flag + _navigator->get_mission_result()->warning = false; + + // trivial case: A mission with length zero cannot be valid + if ((int)mission.count <= 0) { + return false; + } + + bool failed = false; + + // first check if we have a valid position + const bool home_valid = _navigator->home_position_valid(); + const bool home_alt_valid = _navigator->home_alt_valid(); + + if (!home_alt_valid) { + failed = true; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock."); + + } else { + failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint); + } + + const float home_alt = _navigator->get_home_position()->alt; + + // check if all mission item commands are supported + failed = failed || !checkMissionItemValidity(mission); + failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints); + failed = failed || !checkGeofence(mission, home_alt, home_valid); + failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid); + + if (_navigator->get_vstatus()->is_vtol) { + failed = failed || !checkVTOL(mission, home_alt, false); + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + failed = failed || !checkRotarywing(mission, home_alt); + + } else { + failed = failed || !checkFixedwing(mission, home_alt, land_start_req); + } + + return !failed; +} + +bool +MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt) +{ + /* + * Perform check and issue feedback to the user + * Mission is only marked as feasible if takeoff check passes + */ + return checkTakeoff(mission, home_alt); +} + +bool +MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req) +{ + /* Perform checks and issue feedback to the user for all checks */ + bool resTakeoff = checkTakeoff(mission, home_alt); + bool resLanding = checkFixedWingLanding(mission, land_start_req); + + /* Mission is only marked as feasible if all checks return true */ + return (resTakeoff && resLanding); +} + +bool +MissionFeasibilityChecker::checkVTOL(const mission_s &mission, float home_alt, bool land_start_req) +{ + /* Perform checks and issue feedback to the user for all checks */ + bool resTakeoff = checkTakeoff(mission, home_alt); + bool resLanding = checkVTOLLanding(mission, land_start_req); + + /* Mission is only marked as feasible if all checks return true */ + return (resTakeoff && resLanding); +} + +bool +MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_alt, bool home_valid) +{ + if (_navigator->get_geofence().isHomeRequired() && !home_valid) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position"); + return false; + } + + /* Check if all mission items are inside the geofence (if we have a valid geofence) */ + if (_navigator->get_geofence().valid()) { + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(missionitem); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (missionitem.altitude_is_relative && !home_valid) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position"); + return false; + } + + // Geofence function checks against home altitude amsl + missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude; + + if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().check(missionitem)) { + + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu", i + 1); + return false; + } + } + } + + return true; +} + +bool +MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid) +{ + /* Check if all waypoints are above the home altitude */ + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + _navigator->get_mission_result()->warning = true; + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + /* reject relative alt without home set */ + if (missionitem.altitude_is_relative && !home_alt_valid && MissionBlock::item_contains_position(missionitem)) { + + _navigator->get_mission_result()->warning = true; + + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt", i + 1); + return false; + + } + + /* calculate the global waypoint altitude */ + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; + + if (home_alt_valid && home_alt > wp_alt && MissionBlock::item_contains_position(missionitem)) { + + _navigator->get_mission_result()->warning = true; + + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home", i + 1); + } + } + + return true; +} + +bool +MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) +{ + // do not allow mission if we find unsupported item + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + // not supposed to happen unless the datamanager can't access the SD card, etc. + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card"); + return false; + } + + // check if we find unsupported items and reject mission if so + if (missionitem.nav_cmd != NAV_CMD_IDLE && + missionitem.nav_cmd != NAV_CMD_WAYPOINT && + missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && + missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + missionitem.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + missionitem.nav_cmd != NAV_CMD_LAND && + missionitem.nav_cmd != NAV_CMD_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT && + missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_VTOL_LAND && + missionitem.nav_cmd != NAV_CMD_DELAY && + missionitem.nav_cmd != NAV_CMD_CONDITION_GATE && + missionitem.nav_cmd != NAV_CMD_DO_JUMP && + missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && + missionitem.nav_cmd != NAV_CMD_DO_SET_HOME && + missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && + missionitem.nav_cmd != NAV_CMD_DO_LAND_START && + missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE && + missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && + missionitem.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && + missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && + missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { + + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1), + (int)missionitem.nav_cmd); + return false; + } + + /* Check non navigation item */ + if (missionitem.nav_cmd == NAV_CMD_DO_SET_SERVO) { + + /* check actuator number */ + if (missionitem.params[0] < 0 || missionitem.params[0] > 5) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Actuator number %d is out of bounds 0..5", + (int)missionitem.params[0]); + return false; + } + + /* check actuator value */ + if (missionitem.params[1] < -PWM_DEFAULT_MAX || missionitem.params[1] > PWM_DEFAULT_MAX) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX", (int)missionitem.params[1]); + return false; + } + } + + // check if the mission starts with a land command while the vehicle is landed + if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->get_land_detected()->landed) { + + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing"); + return false; + } + } + + return true; +} + +bool +MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt) +{ + bool has_takeoff = false; + bool takeoff_first = false; + int takeoff_index = -1; + + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + // look for a takeoff waypoint + if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { + // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius + // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air + + float takeoff_alt = missionitem.altitude_is_relative + ? missionitem.altitude + : missionitem.altitude - home_alt; + + // check if we should use default acceptance radius + float acceptance_radius = _navigator->get_default_acceptance_radius(); + + if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { + acceptance_radius = missionitem.acceptance_radius; + } + + if (takeoff_alt - 1.0f < acceptance_radius) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!"); + return false; + } + + // tell that mission has a takeoff waypoint + has_takeoff = true; + + // tell that a takeoff waypoint is the first "waypoint" + // mission item + if (i == 0) { + takeoff_first = true; + + } else if (takeoff_index == -1) { + // stores the index of the first takeoff waypoint + takeoff_index = i; + } + } + } + + if (takeoff_index != -1) { + // checks if all the mission items before the first takeoff waypoint + // are not waypoints or position-related items; + // this means that, before a takeoff waypoint, one can set + // one of the bellow mission items + for (size_t i = 0; i < (size_t)takeoff_index; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + takeoff_first = !(missionitem.nav_cmd != NAV_CMD_IDLE && + missionitem.nav_cmd != NAV_CMD_DELAY && + missionitem.nav_cmd != NAV_CMD_DO_JUMP && + missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && + missionitem.nav_cmd != NAV_CMD_DO_SET_HOME && + missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && + missionitem.nav_cmd != NAV_CMD_DO_LAND_START && + missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL && + missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE && + missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE && + missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && + missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && + missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE && + missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && + missionitem.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && + missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && + missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION); + } + } + + if (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) { + // check for a takeoff waypoint, after the above conditions have been met + // MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission + // while the vehicle is flying and it does not require a takeoff waypoint + if (!has_takeoff) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff waypoint required."); + return false; + + } else if (!takeoff_first) { + // check if the takeoff waypoint is the first waypoint item on the mission + // i.e, an item with position/attitude change modification + // if it is not, the mission should be rejected + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item"); + return false; + } + } + + // all checks have passed + return true; +} + +bool +MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool land_start_req) +{ + /* Go through all mission items and search for a landing waypoint + * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ + + bool landing_valid = false; + + bool land_start_found = false; + size_t do_land_start_index = 0; + size_t landing_approach_index = 0; + + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + // if DO_LAND_START found then require valid landing AFTER + if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { + if (land_start_found) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start."); + return false; + + } else { + land_start_found = true; + do_land_start_index = i; + } + } + + if (missionitem.nav_cmd == NAV_CMD_LAND) { + mission_item_s missionitem_previous {}; + + if (i > 0) { + landing_approach_index = i - 1; + + if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (MissionBlock::item_contains_position(missionitem_previous)) { + + uORB::SubscriptionData landing_status{ORB_ID(position_controller_landing_status)}; + + const bool landing_status_valid = (landing_status.get().timestamp > 0); + const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, + missionitem.lat, missionitem.lon); + + if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) { + /* Last wp is before flare region */ + + const float delta_altitude = missionitem.altitude - missionitem_previous.altitude; + + if (delta_altitude < 0) { + + const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement; + const float slope_angle_rad = landing_status.get().slope_angle_rad; + const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, + horizontal_slope_displacement, slope_angle_rad); + + if (missionitem_previous.altitude > slope_alt_req + 1.0f) { + /* Landing waypoint is above altitude of slope at the given waypoint distance (with small tolerance for floating point discrepancies) */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach."); + + const float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, + missionitem.altitude, horizontal_slope_displacement, slope_angle_rad); + + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.", + (int)ceilf(slope_alt_req - missionitem_previous.altitude), + (int)ceilf(wp_distance_req - wp_distance)); + + return false; + } + + } else { + /* Landing waypoint is above last waypoint */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint."); + return false; + } + + } else { + /* Last wp is in flare region */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare."); + return false; + } + + landing_valid = true; + + } else { + // mission item before land doesn't have a position + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach."); + return false; + } + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint."); + return false; + } + + } else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (land_start_found && do_land_start_index < i) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Mission rejected: land start item before RTL item not possible."); + return false; + } + } + } + + if (land_start_req && !land_start_found) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required."); + return false; + } + + if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start."); + return false; + } + + /* No landing waypoints or no waypoints */ + return true; +} + +bool +MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_start_req) +{ + /* Go through all mission items and search for a landing waypoint + * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ + + bool land_start_found = false; + size_t do_land_start_index = 0; + size_t landing_approach_index = 0; + + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); + + if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + // if DO_LAND_START found then require valid landing AFTER + if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { + if (land_start_found) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start."); + return false; + + } else { + land_start_found = true; + do_land_start_index = i; + } + } + + if (missionitem.nav_cmd == NAV_CMD_LAND || missionitem.nav_cmd == NAV_CMD_VTOL_LAND) { + mission_item_s missionitem_previous {}; + + if (i > 0) { + landing_approach_index = i - 1; + + if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint."); + return false; + } + + } else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (land_start_found && do_land_start_index < i) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Mission rejected: land start item before RTL item not possible."); + return false; + } + } + } + + if (land_start_req && !land_start_found) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required."); + return false; + } + + if (land_start_found && (do_land_start_index > landing_approach_index)) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start."); + return false; + } + + /* No landing waypoints or no waypoints */ + return true; +} + +bool +MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance) +{ + if (max_distance <= 0.0f) { + /* param not set, check is ok */ + return true; + } + + /* find first waypoint (with lat/lon) item in datamanager */ + for (size_t i = 0; i < mission.count; i++) { + + struct mission_item_s mission_item {}; + + if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) { + /* error reading, mission is invalid */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission."); + return false; + } + + /* check only items with valid lat/lon */ + if (!MissionBlock::item_contains_position(mission_item)) { + continue; + } + + /* check distance from current position to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + + if (dist_to_1wp < max_distance) { + + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "First waypoint too far away: %dm, %d max", + (int)dist_to_1wp, (int)max_distance); + + _navigator->get_mission_result()->warning = true; + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + return true; +} + +bool +MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance) +{ + if (max_distance <= 0.0f) { + /* param not set, check is ok */ + return true; + } + + double last_lat = (double)NAN; + double last_lon = (double)NAN; + int last_cmd = 0; + + /* Go through all waypoints */ + for (size_t i = 0; i < mission.count; i++) { + + struct mission_item_s mission_item {}; + + if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) { + /* error reading, mission is invalid */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission."); + return false; + } + + /* check only items with valid lat/lon */ + if (!MissionBlock::item_contains_position(mission_item)) { + continue; + } + + /* Compare it to last waypoint if already available. */ + if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) { + + /* check distance from current position to item */ + const float dist_between_waypoints = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + last_lat, last_lon); + + + if (dist_between_waypoints > max_distance) { + /* distance between waypoints is too high */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Distance between waypoints too far: %d meters, %d max.", + (int)dist_between_waypoints, (int)max_distance); + + _navigator->get_mission_result()->warning = true; + return false; + + /* do not allow waypoints that are literally on top of each other */ + + /* and do not allow condition gates that are at the same position as a navigation waypoint */ + + } else if (dist_between_waypoints < 0.05f && + (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || last_cmd == NAV_CMD_CONDITION_GATE)) { + + /* Waypoints and gate are at the exact same position, which indicates an + * invalid mission and makes calculating the direction from one waypoint + * to another impossible. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Distance between waypoint and gate too close: %d meters", + (int)dist_between_waypoints); + + _navigator->get_mission_result()->warning = true; + return false; + } + } + + last_lat = mission_item.lat; + last_lon = mission_item.lon; + last_cmd = mission_item.nav_cmd; + } + + /* We ran through all waypoints and have not found any distances between waypoints that are too far. */ + return true; +} diff --git a/src/prometheus_px4/src/modules/navigator/mission_feasibility_checker.h b/src/prometheus_px4/src/modules/navigator/mission_feasibility_checker.h new file mode 100644 index 0000000..6091f31 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/mission_feasibility_checker.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 mission_feasibility_checker.h + * Provides checks if mission is feasible given the navigation capabilities + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Sander Smeets + * @author Nuno Marques + */ + +#pragma once + +#include +#include + +class Geofence; +class Navigator; + +class MissionFeasibilityChecker +{ +private: + Navigator *_navigator{nullptr}; + + /* Checks for all airframes */ + bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid); + + bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid); + + bool checkMissionItemValidity(const mission_s &mission); + + bool checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance); + bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance); + + bool checkTakeoff(const mission_s &mission, float home_alt); + + /* Checks specific to fixedwing airframes */ + bool checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req); + bool checkFixedWingLanding(const mission_s &mission, bool land_start_req); + + /* Checks specific to rotarywing airframes */ + bool checkRotarywing(const mission_s &mission, float home_alt); + + /* Checks specific to VTOL airframes */ + bool checkVTOL(const mission_s &mission, float home_alt, bool land_start_req); + bool checkVTOLLanding(const mission_s &mission, bool land_start_req); + +public: + MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {} + ~MissionFeasibilityChecker() = default; + + MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete; + MissionFeasibilityChecker &operator=(const MissionFeasibilityChecker &) = delete; + + /* + * Returns true if mission is feasible and false otherwise + */ + bool checkMissionFeasible(const mission_s &mission, + float max_distance_to_1st_waypoint, float max_distance_between_waypoints, + bool land_start_req); + +}; diff --git a/src/prometheus_px4/src/modules/navigator/mission_params.c b/src/prometheus_px4/src/modules/navigator/mission_params.c new file mode 100644 index 0000000..56ebdd3 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/mission_params.c @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 mission_params.c + * + * Parameters for mission. + * + * @author Julian Oes + */ + +/* + * Mission parameters, accessible via MAVLink + */ + +/** + * Take-off altitude + * + * This is the minimum altitude the system will take off to. + * + * @unit m + * @min 0 + * @max 80 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); + +/** + * Take-off waypoint required + * + * If set, the mission feasibility checker will check for a takeoff waypoint on the mission. + * + * @boolean + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_TAKEOFF_REQ, 0); + +/** + * Minimum Loiter altitude + * + * This is the minimum altitude the system will always obey. The intent is to stay out of ground effect. + * set to -1, if there shouldn't be a minimum loiter altitude + * + * @unit m + * @min -1 + * @max 80 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_LTRMIN_ALT, -1.0f); + +/** + * Maximal horizontal distance from home to first waypoint + * + * Failsafe check to prevent running mission stored from previous flight at a new takeoff location. + * Set a value of zero or less to disable. The mission will not be started if the current + * waypoint is more distant than MIS_DIS_1WP from the home position. + * + * @unit m + * @min 0 + * @max 10000 + * @decimal 1 + * @increment 100 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900); + +/** + * Maximal horizontal distance between waypoint + * + * Failsafe check to prevent running missions which are way too big. + * Set a value of zero or less to disable. The mission will not be started if any distance between + * two subsequent waypoints is greater than MIS_DIST_WPS. + * + * @unit m + * @min 0 + * @max 10000 + * @decimal 1 + * @increment 100 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_DIST_WPS, 900); + +/** +* Enable yaw control of the mount. (Only affects multicopters and ROI mission items) +* +* If enabled, yaw commands will be sent to the mount and the vehicle will follow its heading towards the flight direction. +* If disabled, the vehicle will yaw towards the ROI. +* +* @value 0 Disable +* @value 1 Enable +* @min 0 +* @max 1 +* @group Mission +*/ +PARAM_DEFINE_INT32(MIS_MNT_YAW_CTL, 0); + +/** + * Time in seconds we wait on reaching target heading at a waypoint if it is forced. + * + * If set > 0 it will ignore the target heading for normal waypoint acceptance. If the + * waypoint forces the heading the timeout will matter. For example on VTOL forwards transition. + * Mainly useful for VTOLs that have less yaw authority and might not reach target + * yaw in wind. Disabled by default. + * + * @unit s + * @min -1 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f); + +/** + * Max yaw error in degrees needed for waypoint heading acceptance. + * + * @unit deg + * @min 0 + * @max 90 + * @decimal 1 + * @increment 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f); diff --git a/src/prometheus_px4/src/modules/navigator/navigation.h b/src/prometheus_px4/src/modules/navigator/navigation.h new file mode 100644 index 0000000..5c6251d --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/navigation.h @@ -0,0 +1,246 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 navigation.h + * Definition of a mission consisting of mission items. + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +#if defined(MEMORY_CONSTRAINED_SYSTEM) +# define NUM_MISSIONS_SUPPORTED 50 +#elif defined(__PX4_POSIX) +# define NUM_MISSIONS_SUPPORTED (UINT16_MAX-1) // This is allocated as needed. +#else +# define NUM_MISSIONS_SUPPORTED 2000 // This allocates a file of around 181 kB on the SD card. +#endif + +#define NAV_EPSILON_POSITION 0.001f /**< Anything smaller than this is considered zero */ + +/* compatible to mavlink MAV_CMD */ +enum NAV_CMD { + NAV_CMD_IDLE = 0, + NAV_CMD_WAYPOINT = 16, + NAV_CMD_LOITER_UNLIMITED = 17, + NAV_CMD_LOITER_TIME_LIMIT = 19, + NAV_CMD_RETURN_TO_LAUNCH = 20, + NAV_CMD_LAND = 21, + NAV_CMD_TAKEOFF = 22, + NAV_CMD_LOITER_TO_ALT = 31, + NAV_CMD_DO_FOLLOW_REPOSITION = 33, + NAV_CMD_VTOL_TAKEOFF = 84, + NAV_CMD_VTOL_LAND = 85, + NAV_CMD_DELAY = 93, + NAV_CMD_DO_JUMP = 177, + NAV_CMD_DO_CHANGE_SPEED = 178, + NAV_CMD_DO_SET_HOME = 179, + NAV_CMD_DO_SET_SERVO = 183, + NAV_CMD_DO_LAND_START = 189, + NAV_CMD_DO_SET_ROI_LOCATION = 195, + NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196, + NAV_CMD_DO_SET_ROI_NONE = 197, + NAV_CMD_DO_CONTROL_VIDEO = 200, + NAV_CMD_DO_SET_ROI = 201, + NAV_CMD_DO_DIGICAM_CONTROL = 203, + NAV_CMD_DO_MOUNT_CONFIGURE = 204, + NAV_CMD_DO_MOUNT_CONTROL = 205, + NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214, + NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206, + NAV_CMD_OBLIQUE_SURVEY = 260, + NAV_CMD_SET_CAMERA_MODE = 530, + NAV_CMD_SET_CAMERA_ZOOM = 531, + NAV_CMD_SET_CAMERA_FOCUS = 532, + NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000, + NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001, + NAV_CMD_IMAGE_START_CAPTURE = 2000, + NAV_CMD_IMAGE_STOP_CAPTURE = 2001, + NAV_CMD_DO_TRIGGER_CONTROL = 2003, + NAV_CMD_VIDEO_START_CAPTURE = 2500, + NAV_CMD_VIDEO_STOP_CAPTURE = 2501, + NAV_CMD_DO_VTOL_TRANSITION = 3000, + NAV_CMD_FENCE_RETURN_POINT = 5000, + NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION = 5001, + NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION = 5002, + NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003, + NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004, + NAV_CMD_CONDITION_GATE = 4501, + NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */ +}; + +enum ORIGIN { + ORIGIN_MAVLINK = 0, + ORIGIN_ONBOARD +}; + +/* compatible to mavlink MAV_FRAME */ +enum NAV_FRAME { + NAV_FRAME_GLOBAL = 0, + NAV_FRAME_LOCAL_NED = 1, + NAV_FRAME_MISSION = 2, + NAV_FRAME_GLOBAL_RELATIVE_ALT = 3, + NAV_FRAME_LOCAL_ENU = 4, + NAV_FRAME_GLOBAL_INT = 5, + NAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, + NAV_FRAME_LOCAL_OFFSET_NED = 7, + NAV_FRAME_BODY_NED = 8, + NAV_FRAME_BODY_OFFSET_NED = 9, + NAV_FRAME_GLOBAL_TERRAIN_ALT = 10, + NAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 +}; + +/** + * @addtogroup topics + * @{ + */ + +/** + * Global position setpoint in WGS84 coordinates. + * + * This is the position the MAV is heading towards. If it is of type loiter, + * the MAV is circling around it with the given loiter radius in meters. + * + * Corresponds to one of the DM_KEY_WAYPOINTS_OFFBOARD_* dataman items + */ + +// Mission Item structure +// We explicitly handle struct padding to ensure consistency between in memory and on disk formats across different platforms, toolchains, etc +// The use of #pragma pack is avoided to prevent the possibility of unaligned memory accesses. + +#if (__GNUC__ >= 5) || __clang__ +// Disabled in GCC 4.X as the warning doesn't seem to "pop" correctly +#pragma GCC diagnostic push +#pragma GCC diagnostic error "-Wpadded" +#endif // GCC >= 5 || Clang + +struct mission_item_s { + double lat; /**< latitude in degrees */ + double lon; /**< longitude in degrees */ + union { + struct { + union { + float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ + float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */ + }; + float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ + float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */ + float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ + float ___lat_float; /**< padding */ + float ___lon_float; /**< padding */ + float altitude; /**< altitude in meters (AMSL) */ + }; + float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/ + }; + + uint16_t nav_cmd; /**< navigation command */ + + int16_t do_jump_mission_index; /**< index where the do jump will go to */ + uint16_t do_jump_repeat_count; /**< how many times do jump needs to be done */ + + union { + uint16_t do_jump_current_count; /**< count how many times the jump has been done */ + uint16_t vertex_count; /**< Polygon vertex count (geofence) */ + uint16_t land_precision; /**< Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing, 2 = required precision landing (with search) */ + }; + + struct { + uint16_t frame : 4, /**< mission frame */ + origin : 3, /**< how the mission item was generated */ + loiter_exit_xtrack : 1, /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */ + force_heading : 1, /**< heading needs to be reached */ + altitude_is_relative : 1, /**< true if altitude is relative from start point */ + autocontinue : 1, /**< true if next waypoint should follow after this one */ + vtol_back_transition : 1, /**< part of the vtol back transition sequence */ + _padding0 : 4; /**< padding remaining unused bits */ + }; + + uint8_t _padding1[2]; /**< padding struct size to alignment boundary */ +}; + +/** + * dataman housekeeping information for a specific item. + * Corresponds to the first dataman entry of DM_KEY_FENCE_POINTS and DM_KEY_SAFE_POINTS + */ +struct mission_stats_entry_s { + uint16_t num_items; /**< total number of items stored (excluding this one) */ + uint16_t update_counter; /**< This counter is increased when (some) items change (this can wrap) */ +}; + +/** + * Geofence vertex point. + * Corresponds to the DM_KEY_FENCE_POINTS dataman item + */ +struct mission_fence_point_s { + double lat; + double lon; + float alt; + + union { + uint16_t vertex_count; /**< number of vertices in this polygon */ + float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */ + }; + + uint16_t nav_cmd; /**< navigation command (one of MAV_CMD_NAV_FENCE_*) */ + uint8_t frame; /**< MAV_FRAME */ + + uint8_t _padding0[5]; /**< padding struct size to alignment boundary */ +}; + +/** + * Safe Point (Rally Point). + * Corresponds to the DM_KEY_SAFE_POINTS dataman item + */ +struct mission_safe_point_s { + double lat; + double lon; + float alt; + uint8_t frame; /**< MAV_FRAME */ + + uint8_t _padding0[3]; /**< padding struct size to alignment boundary */ +}; + +#if (__GNUC__ >= 5) || __clang__ +#pragma GCC diagnostic pop +#endif // GCC >= 5 || Clang + +#include + +/** + * @} + */ diff --git a/src/prometheus_px4/src/modules/navigator/navigator.h b/src/prometheus_px4/src/modules/navigator/navigator.h new file mode 100644 index 0000000..3b52358 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/navigator.h @@ -0,0 +1,432 @@ +/*************************************************************************** + * + * Copyright (c) 2013-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 navigator.h + * Helper class to access missions + * @author Julian Oes + * @author Anton Babushkin + * @author Thomas Gubler + * @author Lorenz Meier + */ + +#pragma once + +#include "enginefailure.h" +#include "follow_target.h" +#include "geofence.h" +#include "gpsfailure.h" +#include "land.h" +#include "precland.h" +#include "loiter.h" +#include "mission.h" +#include "navigator_mode.h" +#include "rtl.h" +#include "takeoff.h" + +#include "navigation.h" + +#include "GeofenceBreachAvoidance/geofence_breach_avoidance.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +/** + * Number of navigation modes that need on_active/on_inactive calls + */ +#define NAVIGATOR_MODE_ARRAY_SIZE 9 + +class Navigator : public ModuleBase, public ModuleParams +{ +public: + Navigator(); + ~Navigator() override; + + Navigator(const Navigator &) = delete; + Navigator operator=(const Navigator &) = delete; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static Navigator *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; + + /** + * Load fence from file + */ + void load_fence_from_file(const char *filename); + + void publish_vehicle_cmd(vehicle_command_s *vcmd); + + /** + * Generate an artificial traffic indication + * + * @param distance Horizontal distance to this vehicle + * @param direction Direction in earth frame from this vehicle in radians + * @param traffic_heading Travel direction of the traffic in earth frame in radians + * @param altitude_diff Altitude difference, positive is up + * @param hor_velocity Horizontal velocity of traffic, in m/s + * @param ver_velocity Vertical velocity of traffic, in m/s + * @param emitter_type, Type of vehicle, as a number + */ + void fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff, + float hor_velocity, float ver_velocity, int emitter_type); + + /** + * Check nearby traffic for potential collisions + */ + void check_traffic(); + + /** + * Setters + */ + void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } + void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } + void set_mission_result_updated() { _mission_result_updated = true; } + + /** + * Getters + */ + struct home_position_s *get_home_position() { return &_home_pos; } + struct mission_result_s *get_mission_result() { return &_mission_result; } + struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; } + struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; } + struct vehicle_global_position_s *get_global_position() { return &_global_pos; } + struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } + struct vehicle_local_position_s *get_local_position() { return &_local_pos; } + struct vehicle_status_s *get_vstatus() { return &_vstatus; } + PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */ + + const vehicle_roi_s &get_vroi() { return _vroi; } + void reset_vroi() { _vroi = {}; } + + bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); } + bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos && _home_pos.valid_lpos); } + + Geofence &get_geofence() { return _geofence; } + + bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } + float get_loiter_radius() { return _param_nav_loiter_rad.get(); } + + /** + * Returns the default acceptance radius defined by the parameter + */ + float get_default_acceptance_radius(); + + /** + * Get the acceptance radius + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(); + + /** + * Get the default altitude acceptance radius (i.e. from parameters) + * + * @return the distance from the target altitude before considering the waypoint reached + */ + float get_default_altitude_acceptance_radius(); + + /** + * Get the altitude acceptance radius + * + * @return the distance from the target altitude before considering the waypoint reached + */ + float get_altitude_acceptance_radius(); + + /** + * Get the cruising speed + * + * @return the desired cruising speed for this mission + */ + float get_cruising_speed(); + + /** + * Set the cruising speed + * + * Passing a negative value or leaving the parameter away will reset the cruising speed + * to its default value. + * + * For VTOL: sets cruising speed for current mode only (multirotor or fixed-wing). + * + */ + void set_cruising_speed(float speed = -1.0f); + + /** + * Reset cruising speed to default values + * + * For VTOL: resets both cruising speeds. + */ + void reset_cruising_speed(); + + /** + * Set triplets to invalid + */ + void reset_triplets(); + + /** + * Set position setpoint to safe defaults + */ + void reset_position_setpoint(position_setpoint_s &sp); + + /** + * Get the target throttle + * + * @return the desired throttle for this mission + */ + float get_cruising_throttle(); + + /** + * Set the target throttle + */ + void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; } + + /** + * Get the yaw acceptance given the current mission item + * + * @param mission_item_yaw the yaw to use in case the controller-derived radius is finite + * + * @return the yaw at which the next waypoint should be used or NaN if the yaw at a waypoint + * should be ignored + */ + float get_yaw_acceptance(float mission_item_yaw); + + + orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } + + void increment_mission_instance_count() { _mission_result.instance_count++; } + int mission_instance_count() const { return _mission_result.instance_count; } + + void set_mission_failure(const char *reason); + + void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; } + + bool getMissionLandingInProgress() { return _mission_landing_in_progress; } + + bool is_planned_mission() const { return _navigation_mode == &_mission; } + bool on_mission_landing() { return _mission.landing(); } + bool start_mission_landing() { return _mission.land_start(); } + bool get_mission_start_land_available() { return _mission.get_land_start_available(); } + int get_mission_landing_index() { return _mission.get_land_start_index(); } + double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); } + double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); } + float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); } + + double get_mission_landing_lat() { return _mission.get_landing_lat(); } + double get_mission_landing_lon() { return _mission.get_landing_lon(); } + float get_mission_landing_alt() { return _mission.get_landing_alt(); } + + // RTL + bool mission_landing_required() { return _rtl.rtl_type() == RTL::RTL_LAND; } + int rtl_type() { return _rtl.rtl_type(); } + bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } + + bool abort_landing(); + + void geofence_breach_check(bool &have_geofence_position_data); + + // Param access + float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); } + float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); } + bool get_takeoff_required() const { return _param_mis_takeoff_req.get(); } + float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } + float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } + + float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } + float get_vtol_reverse_delay() const { return _param_reverse_delay; } + + bool force_vtol(); + + void acquire_gimbal_control(); + void release_gimbal_control(); + +private: + DEFINE_PARAMETERS( + (ParamFloat) _param_nav_loiter_rad, /**< loiter radius for fixedwing */ + (ParamFloat) _param_nav_acc_rad, /**< acceptance for takeoff */ + (ParamFloat) + _param_nav_fw_alt_rad, /**< acceptance radius for fixedwing altitude */ + (ParamFloat) + _param_nav_fw_altl_rad, /**< acceptance radius for fixedwing altitude before landing*/ + (ParamFloat) + _param_nav_mc_alt_rad, /**< acceptance radius for multicopter altitude */ + (ParamInt) _param_nav_force_vt, /**< acceptance radius for multicopter altitude */ + (ParamInt) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */ + (ParamFloat) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/ + (ParamFloat) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/ + + // non-navigator parameters + // Mission (MIS_*) + (ParamFloat) _param_mis_ltrmin_alt, + (ParamFloat) _param_mis_takeoff_alt, + (ParamBool) _param_mis_takeoff_req, + (ParamFloat) _param_mis_yaw_tmt, + (ParamFloat) _param_mis_yaw_err + ) + + int _local_pos_sub{-1}; + int _mission_sub{-1}; + int _vehicle_status_sub{-1}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::Subscription _gps_pos_sub{ORB_ID(vehicle_gps_position)}; /**< gps position subscription */ + uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ + uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */ + uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */ + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */ + + uORB::SubscriptionData _position_controller_status_sub{ORB_ID(position_controller_status)}; + + uORB::Publication _geofence_result_pub{ORB_ID(geofence_result)}; + uORB::Publication _mission_result_pub{ORB_ID(mission_result)}; + uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; + uORB::Publication _vehicle_roi_pub{ORB_ID(vehicle_roi)}; + + orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ + + uORB::Publication _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; + + // Subscriptions + home_position_s _home_pos{}; /**< home position for RTL */ + mission_result_s _mission_result{}; + vehicle_global_position_s _global_pos{}; /**< global vehicle position */ + vehicle_gps_position_s _gps_pos{}; /**< gps position */ + vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */ + vehicle_local_position_s _local_pos{}; /**< local vehicle position */ + vehicle_status_s _vstatus{}; /**< vehicle status */ + + uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/ + + // Publications + geofence_result_s _geofence_result{}; + position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ + position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */ + position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ + vehicle_roi_s _vroi{}; /**< vehicle ROI */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Geofence _geofence; /**< class that handles the geofence */ + bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */ + GeofenceBreachAvoidance _gf_breach_avoidance; + hrt_abstime _last_geofence_check = 0; + + bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */ + bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ + bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ + + NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ + Mission _mission; /**< class that handles the missions */ + Loiter _loiter; /**< class that handles loiter */ + Takeoff _takeoff; /**< class for handling takeoff commands */ + Land _land; /**< class for handling land commands */ + PrecLand _precland; /**< class for handling precision land commands */ + RTL _rtl; /**< class that handles RTL */ + EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ + GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ + FollowTarget _follow_target; + + NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ + + param_t _handle_back_trans_dec_mss{PARAM_INVALID}; + param_t _handle_reverse_delay{PARAM_INVALID}; + param_t _handle_mpc_jerk_auto{PARAM_INVALID}; + param_t _handle_mpc_acc_hor{PARAM_INVALID}; + + float _param_back_trans_dec_mss{0.f}; + float _param_reverse_delay{0.f}; + float _param_mpc_jerk_auto{4.f}; /**< initialized with the default jerk auto value to prevent division by 0 if the parameter is accidentally set to 0 */ + float _param_mpc_acc_hor{3.f}; /**< initialized with the default horizontal acc value to prevent division by 0 if the parameter is accidentally set to 0 */ + + float _mission_cruising_speed_mc{-1.0f}; + float _mission_cruising_speed_fw{-1.0f}; + float _mission_throttle{NAN}; + + bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern + // if mission mode is inactive, this flag will be cleared after 2 seconds + + // update subscriptions + void params_update(); + + /** + * Publish a new position setpoint triplet for position controllers + */ + void publish_position_setpoint_triplet(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + + void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); +}; diff --git a/src/prometheus_px4/src/modules/navigator/navigator_main.cpp b/src/prometheus_px4/src/modules/navigator/navigator_main.cpp new file mode 100644 index 0000000..9306e69 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/navigator_main.cpp @@ -0,0 +1,1460 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 navigator_main.cpp + * + * Handles mission items, geo fencing and failsafe navigation behavior. + * Published the position setpoint triplet for the position controller. + * + * @author Lorenz Meier + * @author Jean Cyr + * @author Julian Oes + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#include "navigator.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * navigator app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int navigator_main(int argc, char *argv[]); +using namespace time_literals; +namespace navigator +{ +Navigator *g_navigator; +} + +Navigator::Navigator() : + ModuleParams(nullptr), + _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), + _geofence(this), + _gf_breach_avoidance(this), + _mission(this), + _loiter(this), + _takeoff(this), + _land(this), + _precland(this), + _rtl(this), + _engineFailure(this), + _gpsFailure(this), + _follow_target(this) +{ + /* Create a list of our possible navigation types */ + _navigation_mode_array[0] = &_mission; + _navigation_mode_array[1] = &_loiter; + _navigation_mode_array[2] = &_rtl; + _navigation_mode_array[3] = &_engineFailure; + _navigation_mode_array[4] = &_gpsFailure; + _navigation_mode_array[5] = &_takeoff; + _navigation_mode_array[6] = &_land; + _navigation_mode_array[7] = &_precland; + _navigation_mode_array[8] = &_follow_target; + + _handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS"); + _handle_reverse_delay = param_find("VT_B_REV_DEL"); + + _handle_mpc_jerk_auto = param_find("MPC_JERK_AUTO"); + _handle_mpc_acc_hor = param_find("MPC_ACC_HOR"); + + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _mission_sub = orb_subscribe(ORB_ID(mission)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + reset_triplets(); +} + +Navigator::~Navigator() +{ + perf_free(_loop_perf); + orb_unsubscribe(_local_pos_sub); + orb_unsubscribe(_mission_sub); + orb_unsubscribe(_vehicle_status_sub); +} + +void +Navigator::params_update() +{ + updateParams(); + + if (_handle_back_trans_dec_mss != PARAM_INVALID) { + param_get(_handle_back_trans_dec_mss, &_param_back_trans_dec_mss); + } + + if (_handle_reverse_delay != PARAM_INVALID) { + param_get(_handle_reverse_delay, &_param_reverse_delay); + } + + if (_handle_mpc_jerk_auto != PARAM_INVALID) { + param_get(_handle_mpc_jerk_auto, &_param_mpc_jerk_auto); + } + + if (_handle_mpc_acc_hor != PARAM_INVALID) { + param_get(_handle_mpc_acc_hor, &_param_mpc_acc_hor); + } +} + +void +Navigator::run() +{ + bool have_geofence_position_data = false; + + /* Try to load the geofence: + * if /fs/microsd/etc/geofence.txt load from this file */ + struct stat buffer; + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { + PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME); + _geofence.loadFromFile(GEOFENCE_FILENAME); + } + + params_update(); + + /* wakeup source(s) */ + px4_pollfd_struct_t fds[3] {}; + + /* Setup of loop */ + fds[0].fd = _local_pos_sub; + fds[0].events = POLLIN; + fds[1].fd = _vehicle_status_sub; + fds[1].events = POLLIN; + fds[2].fd = _mission_sub; + fds[2].events = POLLIN; + + /* rate-limit position subscription to 20 Hz / 50 ms */ + orb_set_interval(_local_pos_sub, 50); + + while (!should_exit()) { + + /* wait for up to 1000ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + + if (pret == 0) { + /* Let the loop run anyway, don't do `continue` here. */ + + } else if (pret < 0) { + /* this is undesirable but not much we can do - might want to flag unhappy status */ + PX4_ERR("poll error %d, %d", pret, errno); + px4_usleep(10000); + continue; + } + + perf_begin(_loop_perf); + + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus); + + if (fds[2].revents & POLLIN) { + // copy mission to clear any update + mission_s mission; + orb_copy(ORB_ID(mission), _mission_sub, &mission); + } + + /* gps updated */ + if (_gps_pos_sub.updated()) { + _gps_pos_sub.copy(&_gps_pos); + + if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { + have_geofence_position_data = true; + } + } + + /* global position updated */ + if (_global_pos_sub.updated()) { + _global_pos_sub.copy(&_global_pos); + + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + have_geofence_position_data = true; + } + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + params_update(); + } + + _land_detected_sub.update(&_land_detected); + _position_controller_status_sub.update(); + _home_pos_sub.update(&_home_pos); + + while (_vehicle_command_sub.updated()) { + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); + vehicle_command_s cmd{}; + _vehicle_command_sub.copy(&cmd); + + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _vehicle_command_sub.get_last_generation()); + } + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { + + // DO_GO_AROUND is currently handled by the position controller (unacknowledged) + // TODO: move DO_GO_AROUND handling to navigator + publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { + + bool reposition_valid = true; + + if (have_geofence_position_data && + ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN))) { + + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { + + vehicle_global_position_s test_reposition_validity {}; + test_reposition_validity.lat = cmd.param5; + test_reposition_validity.lon = cmd.param6; + + if (PX4_ISFINITE(cmd.param7)) { + test_reposition_validity.alt = cmd.param7; + + } else { + test_reposition_validity.alt = get_global_position()->alt; + } + + reposition_valid = _geofence.check(test_reposition_validity, _gps_pos, _home_pos, + home_position_valid()); + } + } + + if (reposition_valid) { + position_setpoint_triplet_s *rep = get_reposition_triplet(); + position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); + + // store current position as previous position and goal as next + rep->previous.yaw = get_local_position()->heading; + rep->previous.lat = get_global_position()->lat; + rep->previous.lon = get_global_position()->lon; + rep->previous.alt = get_global_position()->alt; + + rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_direction = 1; + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + + // If no argument for ground speed, use default value. + if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) { + rep->current.cruising_speed = get_cruising_speed(); + + } else { + rep->current.cruising_speed = cmd.param1; + } + + rep->current.cruising_throttle = get_cruising_throttle(); + rep->current.acceptance_radius = get_acceptance_radius(); + + // Go on and check which changes had been requested + if (PX4_ISFINITE(cmd.param4)) { + rep->current.yaw = cmd.param4; + rep->current.yaw_valid = true; + + } else { + rep->current.yaw = NAN; + rep->current.yaw_valid = false; + } + + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { + // Position change with optional altitude change + rep->current.lat = cmd.param5; + rep->current.lon = cmd.param6; + + if (PX4_ISFINITE(cmd.param7)) { + rep->current.alt = cmd.param7; + + } else { + rep->current.alt = get_global_position()->alt; + } + + } else if (PX4_ISFINITE(cmd.param7)) { + // Received only a request to change altitude, thus we keep the setpoint + rep->current.lat = curr->current.lat; + rep->current.lon = curr->current.lon; + rep->current.alt = cmd.param7; + + } else { + // All three set to NaN - pause vehicle + rep->current.alt = get_global_position()->alt; + + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { + + // For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back + double lat, lon; + float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx); + + // predict braking distance + + const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); + + float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs, + _param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto); + + waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground, + multirotor_braking_distance, &lat, &lon); + rep->current.lat = lat; + rep->current.lon = lon; + rep->current.yaw = get_local_position()->heading; + rep->current.yaw_valid = true; + + } else { + // For fixedwings we can use the current vehicle's position to define the loiter point + rep->current.lat = get_global_position()->lat; + rep->current.lon = get_global_position()->lon; + } + } + + rep->previous.valid = true; + rep->previous.timestamp = hrt_absolute_time(); + + rep->current.valid = true; + rep->current.timestamp = hrt_absolute_time(); + + rep->next.valid = false; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence"); + } + + // CMD_DO_REPOSITION is acknowledged by commander + + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { + position_setpoint_triplet_s *rep = get_takeoff_triplet(); + + // store current position as previous position and goal as next + rep->previous.yaw = get_local_position()->heading; + rep->previous.lat = get_global_position()->lat; + rep->previous.lon = get_global_position()->lon; + rep->previous.alt = get_global_position()->alt; + + rep->current.loiter_radius = get_loiter_radius(); + rep->current.loiter_direction = 1; + rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + + if (home_position_valid()) { + rep->current.yaw = cmd.param4; + + rep->previous.valid = true; + rep->previous.timestamp = hrt_absolute_time(); + + } else { + rep->current.yaw = get_local_position()->heading; + rep->previous.valid = false; + } + + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { + rep->current.lat = cmd.param5; + rep->current.lon = cmd.param6; + + } else { + // If one of them is non-finite set the current global position as target + rep->current.lat = get_global_position()->lat; + rep->current.lon = get_global_position()->lon; + } + + rep->current.alt = cmd.param7; + + rep->current.valid = true; + rep->current.timestamp = hrt_absolute_time(); + + rep->next.valid = false; + + // CMD_NAV_TAKEOFF is acknowledged by commander + + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { + + /* find NAV_CMD_DO_LAND_START in the mission and + * use MAV_CMD_MISSION_START to start the mission there + */ + if (_mission.land_start()) { + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; + vcmd.param1 = _mission.get_land_start_index(); + publish_vehicle_cmd(&vcmd); + + } else { + PX4_WARN("planned mission landing not available"); + } + + publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { + if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) { + if (!_mission.set_current_mission_index(cmd.param1)) { + PX4_WARN("CMD_MISSION_START failed"); + } + } + + // CMD_MISSION_START is acknowledged by commander + + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { + if (cmd.param2 > FLT_EPSILON) { + // XXX not differentiating ground and airspeed yet + set_cruising_speed(cmd.param2); + + } else { + set_cruising_speed(); + + /* if no speed target was given try to set throttle */ + if (cmd.param3 > FLT_EPSILON) { + set_cruising_throttle(cmd.param3 / 100); + + } else { + set_cruising_throttle(); + } + } + + // TODO: handle responses for supported DO_CHANGE_SPEED options? + publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI + || cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI + || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION + || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET + || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE) { + _vroi = {}; + + switch (cmd.command) { + case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: + case vehicle_command_s::VEHICLE_CMD_NAV_ROI: + _vroi.mode = cmd.param1; + break; + + case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION: + _vroi.mode = vehicle_command_s::VEHICLE_ROI_LOCATION; + _vroi.lat = cmd.param5; + _vroi.lon = cmd.param6; + _vroi.alt = cmd.param7; + break; + + case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: + _vroi.mode = vehicle_command_s::VEHICLE_ROI_WPNEXT; + _vroi.pitch_offset = (float)cmd.param5 * M_DEG_TO_RAD_F; + _vroi.roll_offset = (float)cmd.param6 * M_DEG_TO_RAD_F; + _vroi.yaw_offset = (float)cmd.param7 * M_DEG_TO_RAD_F; + break; + + case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: + _vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE; + break; + + default: + _vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE; + break; + } + + _vroi.timestamp = hrt_absolute_time(); + + _vehicle_roi_pub.publish(_vroi); + + publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + } + } + + /* Check for traffic */ + check_traffic(); + + /* Check geofence violation */ + geofence_breach_check(have_geofence_position_data); + + /* Do stuff according to navigation state set by commander */ + NavigatorMode *navigation_mode_new{nullptr}; + + switch (_vstatus.nav_state) { + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; + + _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL); + navigation_mode_new = &_mission; + + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_loiter; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: { + _pos_sp_triplet_published_invalid_once = false; + + const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + switch (rtl_type()) { + case RTL::RTL_LAND: // use mission landing + case RTL::RTL_CLOSEST: + if (rtl_activated) { + if (rtl_type() == RTL::RTL_LAND) { + mavlink_log_info(get_mavlink_log_pub(), "RTL LAND activated"); + + } else { + mavlink_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated"); + } + + } + + if (!rtl_activated && !_rtl.denyMissionLanding() && _rtl.getClimbAndReturnDone() + && get_mission_start_land_available()) { + _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); + + if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED + && !get_land_detected()->landed) { + start_mission_landing(); + } + + navigation_mode_new = &_mission; + + } else { + navigation_mode_new = &_rtl; + } + + break; + + case RTL::RTL_MISSION: + if (_mission.get_land_start_available() && !get_land_detected()->landed) { + // the mission contains a landing spot + _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); + + if (_navigation_mode != &_mission) { + if (_navigation_mode == nullptr) { + // switching from an manual mode, go to landing if not already landing + if (!on_mission_landing()) { + start_mission_landing(); + } + + } else { + // switching from an auto mode, continue the mission from the closest item + _mission.set_closest_item_as_current(); + } + } + + if (rtl_activated) { + mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission"); + } + + navigation_mode_new = &_mission; + + } else { + // fly the mission in reverse if switching from a non-manual mode + _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE); + + if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) && + (! _mission.get_mission_finished()) && + (!get_land_detected()->landed)) { + // determine the closest mission item if switching from a non-mission mode, and we are either not already + // mission mode or the mission waypoints changed. + // The seconds condition is required so that when no mission was uploaded and one is available the closest + // mission item is determined and also that if the user changes the active mission index while rtl is active + // always that waypoint is tracked first. + if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) { + _mission.set_closest_item_as_current(); + } + + if (rtl_activated) { + mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse"); + } + + navigation_mode_new = &_mission; + + } else { + if (rtl_activated) { + mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home"); + } + + navigation_mode_new = &_rtl; + } + } + + break; + + default: + if (rtl_activated) { + mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated"); + } + + navigation_mode_new = &_rtl; + break; + + } + + break; + } + + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_takeoff; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_land; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_precland; + _precland.set_mode(PrecLandMode::Required); + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_engineFailure; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_gpsFailure; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_follow_target; + break; + + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_STAB: + default: + navigation_mode_new = nullptr; + _can_loiter_at_sp = false; + break; + } + + // Do not execute any state machine while we are disarmed + if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + navigation_mode_new = nullptr; + } + + // update the vehicle status + _previous_nav_state = _vstatus.nav_state; + + /* we have a new navigation mode: reset triplet */ + if (_navigation_mode != navigation_mode_new) { + // We don't reset the triplet if we just did an auto-takeoff and are now + // going to loiter. Otherwise, we lose the takeoff altitude and end up lower + // than where we wanted to go. + // + // FIXME: a better solution would be to add reset where they are needed and remove + // this general reset here. + if (!(_navigation_mode == &_takeoff && + navigation_mode_new == &_loiter)) { + reset_triplets(); + } + } + + _navigation_mode = navigation_mode_new; + + /* iterate through navigation modes and set active/inactive for each */ + for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + if (_navigation_mode_array[i]) { + _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); + } + } + + /* if nothing is running, set position setpoint triplet invalid once */ + if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { + _pos_sp_triplet_published_invalid_once = true; + reset_triplets(); + } + + if (_pos_sp_triplet_updated) { + publish_position_setpoint_triplet(); + } + + if (_mission_result_updated) { + publish_mission_result(); + } + + perf_end(_loop_perf); + } +} + +void Navigator::geofence_breach_check(bool &have_geofence_position_data) +{ + + if (have_geofence_position_data && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) { + + const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); + + matrix::Vector2 fence_violation_test_point; + geofence_violation_type_u gf_violation_type{}; + float test_point_bearing; + float test_point_distance; + float vertical_test_point_distance; + + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); + const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); + _gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs); + _gf_breach_avoidance.setClimbRate(-_local_pos.vz); + test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor(); + vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor(); + + } else { + test_point_distance = 2.0f * get_loiter_radius(); + vertical_test_point_distance = 5.0f; + + if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) { + test_point_bearing = pos_ctrl_status.nav_bearing; + + } else { + test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); + } + } + + _gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance); + _gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance); + _gf_breach_avoidance.setTestPointBearing(test_point_bearing); + _gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt); + _gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome()); + _gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome()); + + if (home_position_valid()) { + _gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt); + } + + fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint(); + + gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0), + fence_violation_test_point(1), + _global_pos.alt); + + gf_violation_type.flags.max_altitude_exceeded = !_geofence.isBelowMaxAltitude(_global_pos.alt + + vertical_test_point_distance); + + gf_violation_type.flags.fence_violation = !_geofence.isInsidePolygonOrCircle(fence_violation_test_point(0), + fence_violation_test_point(1), + _global_pos.alt); + + _last_geofence_check = hrt_absolute_time(); + have_geofence_position_data = false; + + _geofence_result.timestamp = hrt_absolute_time(); + _geofence_result.geofence_action = _geofence.getGeofenceAction(); + _geofence_result.home_required = _geofence.isHomeRequired(); + + if (gf_violation_type.value) { + /* inform other apps via the mission result */ + _geofence_result.geofence_violated = true; + + /* Issue a warning about the geofence violation once and only if we are armed */ + if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence"); + + // we have predicted a geofence violation and if the action is to loiter then + // demand a reposition to a location which is inside the geofence + if (_geofence.getGeofenceAction() == geofence_result_s::GF_ACTION_LOITER) { + position_setpoint_triplet_s *rep = get_reposition_triplet(); + + matrix::Vector2 lointer_center_lat_lon; + matrix::Vector2 current_pos_lat_lon(_global_pos.lat, _global_pos.lon); + float loiter_altitude_amsl = _global_pos.alt; + + + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + // the computation of the braking distance does not match the actual braking distance. Until we have a better model + // we set the loiter point to the current position, that will make sure that the vehicle will loiter inside the fence + lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForMultirotor(gf_violation_type, + &_geofence); + + loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForMulticopter(gf_violation_type); + + } else { + + lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForFixedWing(gf_violation_type, &_geofence); + loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForFixedWing(gf_violation_type); + } + + rep->current.timestamp = hrt_absolute_time(); + rep->current.yaw = get_local_position()->heading; + rep->current.yaw_valid = true; + rep->current.lat = lointer_center_lat_lon(0); + rep->current.lon = lointer_center_lat_lon(1); + rep->current.alt = loiter_altitude_amsl; + rep->current.valid = true; + rep->current.loiter_radius = get_loiter_radius(); + rep->current.alt_valid = true; + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + rep->current.loiter_direction = 1; + rep->current.cruising_throttle = get_cruising_throttle(); + rep->current.acceptance_radius = get_acceptance_radius(); + rep->current.cruising_speed = get_cruising_speed(); + + } + + _geofence_violation_warning_sent = true; + } + + } else { + /* inform other apps via the mission result */ + _geofence_result.geofence_violated = false; + + /* Reset the _geofence_violation_warning_sent field */ + _geofence_violation_warning_sent = false; + } + + _geofence_result_pub.publish(_geofence_result); + } +} + +int Navigator::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("navigator", + SCHED_DEFAULT, + SCHED_PRIORITY_NAVIGATION, + 1800, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +Navigator *Navigator::instantiate(int argc, char *argv[]) +{ + Navigator *instance = new Navigator(); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; +} + +int +Navigator::print_status() +{ + PX4_INFO("Running"); + + _geofence.printStatus(); + return 0; +} + +void +Navigator::publish_position_setpoint_triplet() +{ + _pos_sp_triplet.timestamp = hrt_absolute_time(); + _pos_sp_triplet_pub.publish(_pos_sp_triplet); + _pos_sp_triplet_updated = false; +} + +float +Navigator::get_default_acceptance_radius() +{ + return _param_nav_acc_rad.get(); +} + +float +Navigator::get_default_altitude_acceptance_radius() +{ + if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return _param_nav_fw_alt_rad.get(); + + } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + return INFINITY; + + } else { + float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); + + const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); + + if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) + && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { + alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; + } + + return alt_acceptance_radius; + } +} + +float +Navigator::get_altitude_acceptance_radius() +{ + if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next; + + if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { + // Use separate (tighter) altitude acceptance for clean altitude starting point before landing + return _param_nav_fw_altl_rad.get(); + } + } + + return get_default_altitude_acceptance_radius(); +} + +float +Navigator::get_cruising_speed() +{ + /* there are three options: The mission-requested cruise speed, or the current hover / plane speed */ + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) { + return _mission_cruising_speed_mc; + + } else { + return -1.0f; + } + + } else { + if (is_planned_mission() && _mission_cruising_speed_fw > 0.0f) { + return _mission_cruising_speed_fw; + + } else { + return -1.0f; + } + } +} + +void +Navigator::set_cruising_speed(float speed) +{ + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _mission_cruising_speed_mc = speed; + + } else { + _mission_cruising_speed_fw = speed; + } +} + +void +Navigator::reset_cruising_speed() +{ + _mission_cruising_speed_mc = -1.0f; + _mission_cruising_speed_fw = -1.0f; +} + +void +Navigator::reset_triplets() +{ + reset_position_setpoint(_pos_sp_triplet.previous); + reset_position_setpoint(_pos_sp_triplet.current); + reset_position_setpoint(_pos_sp_triplet.next); + + _pos_sp_triplet_updated = true; +} + +void +Navigator::reset_position_setpoint(position_setpoint_s &sp) +{ + sp = position_setpoint_s{}; + sp.timestamp = hrt_absolute_time(); + sp.lat = static_cast(NAN); + sp.lon = static_cast(NAN); + sp.loiter_radius = get_loiter_radius(); + sp.acceptance_radius = get_default_acceptance_radius(); + sp.cruising_speed = get_cruising_speed(); + sp.cruising_throttle = get_cruising_throttle(); + sp.valid = false; + sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE; + sp.disable_weather_vane = false; +} + +float +Navigator::get_cruising_throttle() +{ + /* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */ + if (_mission_throttle > FLT_EPSILON) { + return _mission_throttle; + + } else { + return NAN; + } +} + +float +Navigator::get_acceptance_radius() +{ + float acceptance_radius = get_default_acceptance_radius(); // the value specified in the parameter NAV_ACC_RAD + const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); + + // for fixed-wing and rover, return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance) + if (_vstatus.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && PX4_ISFINITE(pos_ctrl_status.acceptance_radius) && pos_ctrl_status.timestamp != 0) { + + acceptance_radius = math::max(acceptance_radius, pos_ctrl_status.acceptance_radius); + } + + return acceptance_radius; +} + +float +Navigator::get_yaw_acceptance(float mission_item_yaw) +{ + float yaw = mission_item_yaw; + + const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); + + // if yaw_acceptance from position controller is NaN overwrite the mission item yaw such that + // the waypoint can be reached from any direction + if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) { + yaw = pos_ctrl_status.yaw_acceptance; + } + + return yaw; +} + +void +Navigator::load_fence_from_file(const char *filename) +{ + _geofence.loadFromFile(filename); +} + +/** + * Creates a fake traffic measurement with supplied parameters. + * + */ +void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, + float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type) +{ + double lat, lon; + waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat, + &lon); + float alt = get_global_position()->alt + altitude_diff; + + // float vel_n = get_global_position()->vel_n; + // float vel_e = get_global_position()->vel_e; + // float vel_d = get_global_position()->vel_d; + + transponder_report_s tr{}; + tr.timestamp = hrt_absolute_time(); + tr.icao_address = 1234; + tr.lat = lat; // Latitude, expressed as degrees + tr.lon = lon; // Longitude, expressed as degrees + tr.altitude_type = 0; + tr.altitude = alt; + tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians + tr.hor_velocity = hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s + tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up + strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign) - 1); + tr.callsign[sizeof(tr.callsign) - 1] = 0; + tr.emitter_type = emitter_type; // Type from ADSB_EMITTER_TYPE enum + tr.tslc = 2; // Time since last communication in seconds + tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | + transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | + transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE | + (transponder_report_s::ADSB_EMITTER_TYPE_UAV & emitter_type ? 0 : + transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN); // Flags to indicate various statuses including valid data fields + tr.squawk = 6667; + + + + +#ifndef BOARD_HAS_NO_UUID + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + memcpy(tr.uas_id, px4_guid, sizeof(px4_guid_t)); //simulate own GUID +#else + + for (int i = 0; i < PX4_GUID_BYTE_LENGTH ; i++) { + tr.uas_id[i] = 0xe0 + i; //simulate GUID + } + +#endif /* BOARD_HAS_NO_UUID */ + + uORB::Publication tr_pub{ORB_ID(transponder_report)}; + tr_pub.publish(tr); +} + +void Navigator::check_traffic() +{ + double lat = get_global_position()->lat; + double lon = get_global_position()->lon; + float alt = get_global_position()->alt; + + // TODO for non-multirotors predicting the future + // position as accurately as possible will become relevant + // float vel_n = get_global_position()->vel_n; + // float vel_e = get_global_position()->vel_e; + // float vel_d = get_global_position()->vel_d; + + bool changed = _traffic_sub.updated(); + + char uas_id[11]; //GUID of incoming UTM messages + + float NAVTrafficAvoidUnmanned = _param_nav_traff_a_radu.get(); + float NAVTrafficAvoidManned = _param_nav_traff_a_radm.get(); + float horizontal_separation = NAVTrafficAvoidManned; + float vertical_separation = NAVTrafficAvoidManned; + + while (changed) { + + //vehicle_status_s vs{}; + transponder_report_s tr{}; + _traffic_sub.copy(&tr); + + uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | + transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | + transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; + + if ((tr.flags & required_flags) != required_flags) { + changed = _traffic_sub.updated(); + continue; + } + + //convert UAS_id byte array to char array for User Warning + for (int i = 0; i < 5; i++) { + snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", tr.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]); + } + + //Manned/Unmanned Vehicle Seperation Distance + if (tr.emitter_type == transponder_report_s::ADSB_EMITTER_TYPE_UAV) { + horizontal_separation = NAVTrafficAvoidUnmanned; + vertical_separation = NAVTrafficAvoidUnmanned; + } + + float d_hor, d_vert; + get_distance_to_point_global_wgs84(lat, lon, alt, + tr.lat, tr.lon, tr.altitude, &d_hor, &d_vert); + + + // predict final altitude (positive is up) in prediction time frame + float end_alt = tr.altitude + (d_vert / tr.hor_velocity) * tr.ver_velocity; + + // Predict until the vehicle would have passed this system at its current speed + float prediction_distance = d_hor + 1000.0f; + + // If the altitude is not getting close to us, do not calculate + // the horizontal separation. + // Since commercial flights do most of the time keep flight levels + // check for the current and for the predicted flight level. + // we also make the implicit assumption that this system is on the lowest + // flight level close to ground in the + // (end_alt - horizontal_separation < alt) condition. If this system should + // ever be used in normal airspace this implementation would anyway be + // inappropriate as it should be replaced with a TCAS compliant solution. + + if ((fabsf(alt - tr.altitude) < vertical_separation) || ((end_alt - horizontal_separation) < alt)) { + + double end_lat, end_lon; + waypoint_from_heading_and_distance(tr.lat, tr.lon, tr.heading, prediction_distance, &end_lat, &end_lon); + + struct crosstrack_error_s cr; + + if (!get_distance_to_line(&cr, lat, lon, tr.lat, tr.lon, end_lat, end_lon)) { + + if (!cr.past_end && (fabsf(cr.distance) < horizontal_separation)) { + + // direction of traffic in human-readable 0..360 degree in earth frame + int traffic_direction = math::degrees(tr.heading) + 180; + int traffic_seperation = (int)fabsf(cr.distance); + + switch (_param_nav_traff_avoid.get()) { + + case 0: { + /* Ignore */ + PX4_WARN("TRAFFIC %s! dst %d, hdg %d", + tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, + traffic_seperation, + traffic_direction); + break; + } + + case 1: { + /* Warn only */ + mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d", + tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, + traffic_seperation, + traffic_direction); + break; + } + + case 2: { + /* RTL Mode */ + mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d", + tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, + traffic_seperation, + traffic_direction); + + // set the return altitude to minimum + _rtl.set_return_alt_min(true); + + // ask the commander to execute an RTL + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; + publish_vehicle_cmd(&vcmd); + break; + } + + case 3: { + /* Land Mode */ + mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d", + tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, + traffic_seperation, + traffic_direction); + + // ask the commander to land + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; + publish_vehicle_cmd(&vcmd); + break; + + } + + case 4: { + /* Position hold */ + mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d", + tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : uas_id, + traffic_seperation, + traffic_direction); + + // ask the commander to Loiter + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; + publish_vehicle_cmd(&vcmd); + break; + + } + } + } + } + } + + changed = _traffic_sub.updated(); + } +} + +bool +Navigator::abort_landing() +{ + // only abort if currently landing and position controller status updated + bool should_abort = false; + + if (_pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + + if (_pos_ctrl_landing_status_sub.updated()) { + position_controller_landing_status_s landing_status{}; + + // landing status from position controller must be newer than navigator's last position setpoint + if (_pos_ctrl_landing_status_sub.copy(&landing_status)) { + if (landing_status.timestamp > _pos_sp_triplet.timestamp) { + should_abort = landing_status.abort_landing; + } + } + } + } + + return should_abort; +} + +bool +Navigator::force_vtol() +{ + return _vstatus.is_vtol && + (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || _vstatus.in_transition_to_fw) + && _param_nav_force_vt.get(); +} + +int Navigator::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + print_usage("not running"); + return 1; + } + + if (!strcmp(argv[0], "fencefile")) { + get_instance()->load_fence_from_file(GEOFENCE_FILENAME); + return 0; + + } else if (!strcmp(argv[0], "fake_traffic")) { + get_instance()->fake_traffic("LX007", 500, 1.0f, -1.0f, 100.0f, 90.0f, 0.001f, + transponder_report_s::ADSB_EMITTER_TYPE_LIGHT); + get_instance()->fake_traffic("LX55", 1000, 0, 0, 100.0f, 90.0f, 0.001f, transponder_report_s::ADSB_EMITTER_TYPE_SMALL); + get_instance()->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f, + transponder_report_s::ADSB_EMITTER_TYPE_LARGE); + get_instance()->fake_traffic("UAV", 10, 1.0f, -2.0f, 10.0f, 10.0f, 0.01f, transponder_report_s::ADSB_EMITTER_TYPE_UAV); + return 0; + } + + return print_usage("unknown command"); +} + +int navigator_main(int argc, char *argv[]) +{ + return Navigator::main(argc, argv); +} + +void +Navigator::publish_mission_result() +{ + _mission_result.timestamp = hrt_absolute_time(); + + /* lazily publish the mission result only once available */ + _mission_result_pub.publish(_mission_result); + + /* reset some of the flags */ + _mission_result.item_do_jump_changed = false; + _mission_result.item_changed_index = 0; + _mission_result.item_do_jump_remaining = 0; + + _mission_result_updated = false; +} + +void +Navigator::set_mission_failure(const char *reason) +{ + if (!_mission_result.failure) { + _mission_result.failure = true; + set_mission_result_updated(); + mavlink_log_critical(&_mavlink_log_pub, "%s", reason); + } +} + +void +Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) +{ + vcmd->timestamp = hrt_absolute_time(); + vcmd->source_system = _vstatus.system_id; + vcmd->source_component = _vstatus.component_id; + vcmd->target_system = _vstatus.system_id; + vcmd->confirmation = false; + vcmd->from_external = false; + + // The camera commands are not processed on the autopilot but will be + // sent to the mavlink links to other components. + switch (vcmd->command) { + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + case NAV_CMD_VIDEO_START_CAPTURE: + case NAV_CMD_VIDEO_STOP_CAPTURE: + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + break; + + default: + vcmd->target_component = _vstatus.component_id; + break; + } + + _vehicle_cmd_pub.publish(*vcmd); +} + +void +Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result) +{ + vehicle_command_ack_s command_ack = {}; + + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = cmd.command; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + command_ack.from_external = false; + + command_ack.result = result; + command_ack.result_param1 = 0; + command_ack.result_param2 = 0; + + _vehicle_cmd_ack_pub.publish(command_ack); +} + +void +Navigator::acquire_gimbal_control() +{ + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vcmd.param1 = _vstatus.system_id; + vcmd.param2 = _vstatus.component_id; + vcmd.param3 = -1.0f; // Leave unchanged. + vcmd.param4 = -1.0f; // Leave unchanged. + publish_vehicle_cmd(&vcmd); +} + +void +Navigator::release_gimbal_control() +{ + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vcmd.param1 = -3.0f; // Remove control if it had it. + vcmd.param2 = -3.0f; // Remove control if it had it. + vcmd.param3 = -1.0f; // Leave unchanged. + vcmd.param4 = -1.0f; // Leave unchanged. + publish_vehicle_cmd(&vcmd); +} + +int Navigator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Module that is responsible for autonomous flight modes. This includes missions (read from dataman), +takeoff and RTL. +It is also responsible for geofence violation checking. + +### Implementation +The different internal modes are implemented as separate classes that inherit from a common base class `NavigatorMode`. +The member `_navigation_mode` contains the current active mode. + +Navigator publishes position setpoint triplets (`position_setpoint_triplet_s`), which are then used by the position +controller. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("navigator", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND_DESCR("fencefile", "load a geofence file from SD card, stored at etc/geofence.txt"); + PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 4 fake transponder_report_s uORB messages"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} diff --git a/src/prometheus_px4/src/modules/navigator/navigator_mode.cpp b/src/prometheus_px4/src/modules/navigator/navigator_mode.cpp new file mode 100644 index 0000000..6a22426 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/navigator_mode.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 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 navigator_mode.cpp + * + * Base class for different modes in navigator + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "navigator_mode.h" +#include "navigator.h" + +NavigatorMode::NavigatorMode(Navigator *navigator) : + _navigator(navigator) +{ + /* set initial mission items */ + on_inactivation(); + on_inactive(); +} + +void +NavigatorMode::run(bool active) +{ + if (active) { + if (!_active) { + /* first run, reset stay in failsafe flag */ + _navigator->get_mission_result()->stay_in_failsafe = false; + _navigator->set_mission_result_updated(); + on_activation(); + + } else { + /* periodic updates when active */ + on_active(); + } + + } else { + /* periodic updates when inactive */ + if (_active) { + on_inactivation(); + + } else { + on_inactive(); + } + } + + _active = active; +} + +void +NavigatorMode::on_inactive() +{ +} + +void +NavigatorMode::on_inactivation() +{ +} + +void +NavigatorMode::on_activation() +{ + /* invalidate position setpoint by default */ + _navigator->get_position_setpoint_triplet()->current.valid = false; +} + +void +NavigatorMode::on_active() +{ +} diff --git a/src/prometheus_px4/src/modules/navigator/navigator_mode.h b/src/prometheus_px4/src/modules/navigator/navigator_mode.h new file mode 100644 index 0000000..a3a48a9 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/navigator_mode.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 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 navigator_mode.h + * + * Base class for different modes in navigator + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +class Navigator; + +class NavigatorMode +{ +public: + NavigatorMode(Navigator *navigator); + virtual ~NavigatorMode() = default; + NavigatorMode(const NavigatorMode &) = delete; + NavigatorMode operator=(const NavigatorMode &) = delete; + + void run(bool active); + + /** + * This function is called while the mode is inactive + */ + virtual void on_inactive(); + + /** + * This function is called one time when mode becomes active, pos_sp_triplet must be initialized here + */ + virtual void on_activation(); + + /** + * This function is called one time when mode becomes inactive + */ + virtual void on_inactivation(); + + /** + * This function is called while the mode is active + */ + virtual void on_active(); + +protected: + Navigator *_navigator{nullptr}; + +private: + bool _active{false}; +}; diff --git a/src/prometheus_px4/src/modules/navigator/navigator_params.c b/src/prometheus_px4/src/modules/navigator/navigator_params.c new file mode 100644 index 0000000..b3f7678 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/navigator_params.c @@ -0,0 +1,200 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 navigator_params.c + * + * Parameters for navigator in general + * + * @author Julian Oes + * @author Thomas Gubler + */ + +/** + * Loiter radius (FW only) + * + * Default value of loiter radius for missions, Hold mode, Return mode, etc. (fixedwing only). + * + * @unit m + * @min 25 + * @max 1000 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * Acceptance Radius + * + * Default acceptance radius, overridden by acceptance radius of waypoint if set. + * For fixed wing the L1 turning distance is used for horizontal acceptance. + * + * @unit m + * @min 0.05 + * @max 200.0 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); + +/** + * FW Altitude Acceptance Radius + * + * Acceptance radius for fixedwing altitude. + * + * @unit m + * @min 0.05 + * @max 200.0 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_FW_ALT_RAD, 10.0f); + +/** + * FW Altitude Acceptance Radius before a landing + * + * Altitude acceptance used for the last waypoint before a fixed-wing landing. This is usually smaller + * than the standard vertical acceptance because close to the ground higher accuracy is required. + * + * @unit m + * @min 0.05 + * @max 200.0 + * @decimal 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_FW_ALTL_RAD, 5.0f); + +/** + * MC Altitude Acceptance Radius + * + * Acceptance radius for multicopter altitude. + * + * @unit m + * @min 0.05 + * @max 200.0 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 0.8f); + +/** + * Set traffic avoidance mode + * + * Enabling this will allow the system to respond + * to transponder data from e.g. ADSB transponders + * + * @value 0 Disabled + * @value 1 Warn only + * @value 2 Return mode + * @value 3 Land mode + * @value 4 Position Hold mode + * + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_TRAFF_AVOID, 1); + +/** + * Set NAV TRAFFIC AVOID RADIUS MANNED + * + * Defines the Radius where NAV TRAFFIC AVOID is Called + * For Manned Aviation + * + * @unit m + * @min 500 + * + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADM, 500); + +/** + * Set NAV TRAFFIC AVOID RADIUS + * + * Defines the Radius where NAV TRAFFIC AVOID is Called + * For Unmanned Aviation + * + * @unit m + * @min 10 + * @max 500 + * + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_TRAFF_A_RADU, 10); + +/** + * Airfield home Lat + * + * Latitude of airfield home waypoint + * + * @unit deg*1e7 + * @min -900000000 + * @max 900000000 + * @group Data Link Loss + */ +PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); + +/** + * Airfield home Lon + * + * Longitude of airfield home waypoint + * + * @unit deg*1e7 + * @min -1800000000 + * @max 1800000000 + * @group Data Link Loss + */ +PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); + +/** + * Airfield home alt + * + * Altitude of airfield home waypoint + * + * @unit m + * @min -50 + * @decimal 1 + * @increment 0.5 + * @group Data Link Loss + */ +PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); + +/** + * Force VTOL mode takeoff and land + * + * @boolean + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_FORCE_VT, 1); diff --git a/src/prometheus_px4/src/modules/navigator/precland.cpp b/src/prometheus_px4/src/modules/navigator/precland.cpp new file mode 100644 index 0000000..c0ee352 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/precland.cpp @@ -0,0 +1,597 @@ +/**************************************************************************** + * + * Copyright (c) 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 precland.cpp + * + * Helper class to do precision landing with a landing target + * + * @author Nicolas de Palezieux (Sunflower Labs) + */ + +#include "precland.h" +#include "navigator.h" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#define SEC2USEC 1000000.0f + +#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state + +PrecLand::PrecLand(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ + _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); + _handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE"); + + updateParams(); +} + +void +PrecLand::on_activation() +{ + _state = PrecLandState::Start; + _search_cnt = 0; + _last_slewrate_time = 0; + + vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); + + if (!map_projection_initialized(&_map_ref)) { + map_projection_init(&_map_ref, vehicle_local_position->ref_lat, vehicle_local_position->ref_lon); + } + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + pos_sp_triplet->next.valid = false; + pos_sp_triplet->previous.valid = false; + + // Check that the current position setpoint is valid, otherwise land at current position + if (!pos_sp_triplet->current.valid) { + PX4_WARN("Resetting landing position to current position"); + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.timestamp = hrt_absolute_time(); + } + + _sp_pev = matrix::Vector2f(0, 0); + _sp_pev_prev = matrix::Vector2f(0, 0); + _last_slewrate_time = 0; + + switch_to_state_start(); + + _is_activated = true; +} + +void +PrecLand::on_active() +{ + // get new target measurement + _target_pose_updated = _target_pose_sub.update(&_target_pose); + + if (_target_pose_updated) { + _target_pose_valid = true; + } + + if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) { + _target_pose_valid = false; + } + + // stop if we are landed + if (_navigator->get_land_detected()->landed) { + switch_to_state_done(); + } + + switch (_state) { + case PrecLandState::Start: + run_state_start(); + break; + + case PrecLandState::HorizontalApproach: + run_state_horizontal_approach(); + break; + + case PrecLandState::DescendAboveTarget: + run_state_descend_above_target(); + break; + + case PrecLandState::FinalApproach: + run_state_final_approach(); + break; + + case PrecLandState::Search: + run_state_search(); + break; + + case PrecLandState::Fallback: + run_state_fallback(); + break; + + case PrecLandState::Done: + // nothing to do + break; + + default: + // unknown state + break; + } + +} + +void +PrecLand::on_inactivation() +{ + _is_activated = false; +} + +void +PrecLand::updateParams() +{ + ModuleParams::updateParams(); + + if (_handle_param_acceleration_hor != PARAM_INVALID) { + param_get(_handle_param_acceleration_hor, &_param_acceleration_hor); + } + + if (_handle_param_xy_vel_cruise != PARAM_INVALID) { + param_get(_handle_param_xy_vel_cruise, &_param_xy_vel_cruise); + } +} + +void +PrecLand::run_state_start() +{ + // check if target visible and go to horizontal approach + if (switch_to_state_horizontal_approach()) { + return; + } + + if (_mode == PrecLandMode::Opportunistic) { + // could not see the target immediately, so just fall back to normal landing + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to search or fallback landing"); + } + } + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + float dist = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + // check if we've reached the start point + if (dist < _navigator->get_acceptance_radius()) { + if (!_point_reached_time) { + _point_reached_time = hrt_absolute_time(); + } + + // if we don't see the target after 1 second, search for it + if (_param_pld_srch_tout.get() > 0) { + + if (hrt_absolute_time() - _point_reached_time > 2000000) { + if (!switch_to_state_search()) { + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to search or fallback landing"); + } + } + } + + } else { + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to search or fallback landing"); + } + } + } +} + +void +PrecLand::run_state_horizontal_approach() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // check if target visible, if not go to start + if (!check_state_conditions(PrecLandState::HorizontalApproach)) { + PX4_WARN("Lost landing target while landing (horizontal approach)."); + + // Stay at current position for searching for the landing target + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; + + if (!switch_to_state_start()) { + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to fallback landing"); + } + } + + return; + } + + if (check_state_conditions(PrecLandState::DescendAboveTarget)) { + if (!_point_reached_time) { + _point_reached_time = hrt_absolute_time(); + } + + if (hrt_absolute_time() - _point_reached_time > 2000000) { + // if close enough for descent above target go to descend above target + if (switch_to_state_descend_above_target()) { + return; + } + } + + } + + if (hrt_absolute_time() - _state_start_time > STATE_TIMEOUT) { + PX4_ERR("Precision landing took too long during horizontal approach phase."); + + if (switch_to_state_fallback()) { + return; + } + + PX4_ERR("Can't switch to fallback landing"); + } + + float x = _target_pose.x_abs; + float y = _target_pose.y_abs; + + slewrate(x, y); + + // XXX need to transform to GPS coords because mc_pos_control only looks at that + double lat, lon; + map_projection_reproject(&_map_ref, x, y, &lat, &lon); + + pos_sp_triplet->current.lat = lat; + pos_sp_triplet->current.lon = lon; + pos_sp_triplet->current.alt = _approach_alt; + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +PrecLand::run_state_descend_above_target() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // check if target visible + if (!check_state_conditions(PrecLandState::DescendAboveTarget)) { + if (!switch_to_state_final_approach()) { + PX4_WARN("Lost landing target while landing (descending)."); + + // Stay at current position for searching for the target + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; + + if (!switch_to_state_start()) { + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to fallback landing"); + } + } + } + + return; + } + + // XXX need to transform to GPS coords because mc_pos_control only looks at that + double lat, lon; + map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon); + + pos_sp_triplet->current.lat = lat; + pos_sp_triplet->current.lon = lon; + + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +PrecLand::run_state_final_approach() +{ + // nothing to do, will land +} + +void +PrecLand::run_state_search() +{ + // check if we can see the target + if (check_state_conditions(PrecLandState::HorizontalApproach)) { + if (!_target_acquired_time) { + // target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly + _target_acquired_time = hrt_absolute_time(); + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + float new_alt = _navigator->get_global_position()->alt + 1.0f; + pos_sp_triplet->current.alt = new_alt < pos_sp_triplet->current.alt ? new_alt : pos_sp_triplet->current.alt; + _navigator->set_position_setpoint_triplet_updated(); + } + + } + + // stay at that height for a second to allow the vehicle to settle + if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1000000) { + // try to switch to horizontal approach + if (switch_to_state_horizontal_approach()) { + return; + } + } + + // check if search timed out and go to fallback + if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) { + PX4_WARN("Search timed out"); + + if (!switch_to_state_fallback()) { + PX4_ERR("Can't switch to fallback landing"); + } + } +} + +void +PrecLand::run_state_fallback() +{ + // nothing to do, will land +} + +bool +PrecLand::switch_to_state_start() +{ + if (check_state_conditions(PrecLandState::Start)) { + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _navigator->set_position_setpoint_triplet_updated(); + _search_cnt++; + + _point_reached_time = 0; + + _state = PrecLandState::Start; + _state_start_time = hrt_absolute_time(); + return true; + } + + return false; +} + +bool +PrecLand::switch_to_state_horizontal_approach() +{ + if (check_state_conditions(PrecLandState::HorizontalApproach)) { + _approach_alt = _navigator->get_global_position()->alt; + + _point_reached_time = 0; + + _state = PrecLandState::HorizontalApproach; + _state_start_time = hrt_absolute_time(); + return true; + } + + return false; +} + +bool +PrecLand::switch_to_state_descend_above_target() +{ + if (check_state_conditions(PrecLandState::DescendAboveTarget)) { + _state = PrecLandState::DescendAboveTarget; + _state_start_time = hrt_absolute_time(); + return true; + } + + return false; +} + +bool +PrecLand::switch_to_state_final_approach() +{ + if (check_state_conditions(PrecLandState::FinalApproach)) { + _state = PrecLandState::FinalApproach; + _state_start_time = hrt_absolute_time(); + return true; + } + + return false; +} + +bool +PrecLand::switch_to_state_search() +{ + PX4_INFO("Climbing to search altitude."); + vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->current.alt = vehicle_local_position->ref_alt + _param_pld_srch_alt.get(); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _navigator->set_position_setpoint_triplet_updated(); + + _target_acquired_time = 0; + + _state = PrecLandState::Search; + _state_start_time = hrt_absolute_time(); + return true; +} + +bool +PrecLand::switch_to_state_fallback() +{ + PX4_WARN("Falling back to normal land."); + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + _navigator->set_position_setpoint_triplet_updated(); + + _state = PrecLandState::Fallback; + _state_start_time = hrt_absolute_time(); + return true; +} + +bool +PrecLand::switch_to_state_done() +{ + _state = PrecLandState::Done; + _state_start_time = hrt_absolute_time(); + return true; +} + +bool PrecLand::check_state_conditions(PrecLandState state) +{ + vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); + + switch (state) { + case PrecLandState::Start: + return _search_cnt <= _param_pld_max_srch.get(); + + case PrecLandState::HorizontalApproach: + + // if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore + if (_state == PrecLandState::HorizontalApproach) { + if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get() + && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get()) { + // we've reached the position where we last saw the target. If we don't see it now, we need to do something + return _target_pose_valid && _target_pose.abs_pos_valid; + + } else { + // We've seen the target sometime during horizontal approach. + // Even if we don't see it as we're moving towards it, continue approaching last known location + return true; + } + } + + // If we're trying to switch to this state, the target needs to be visible + return _target_pose_updated && _target_pose_valid && _target_pose.abs_pos_valid; + + case PrecLandState::DescendAboveTarget: + + // if we're already in this state, only leave it if target becomes unusable, don't care about horizontall offset to target + if (_state == PrecLandState::DescendAboveTarget) { + // if we're close to the ground, we're more critical of target timeouts so we quickly go into descend + if (check_state_conditions(PrecLandState::FinalApproach)) { + return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s + + } else { + return _target_pose_valid && _target_pose.abs_pos_valid; + } + + } else { + // if not already in this state, need to be above target to enter it + return _target_pose_updated && _target_pose.abs_pos_valid + && fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get() + && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get(); + } + + case PrecLandState::FinalApproach: + return _target_pose_valid && _target_pose.abs_pos_valid + && (_target_pose.z_abs - vehicle_local_position->z) < _param_pld_fappr_alt.get(); + + case PrecLandState::Search: + return true; + + case PrecLandState::Fallback: + return true; + + default: + return false; + } +} + +void PrecLand::slewrate(float &sp_x, float &sp_y) +{ + matrix::Vector2f sp_curr(sp_x, sp_y); + uint64_t now = hrt_absolute_time(); + + float dt = (now - _last_slewrate_time); + + if (dt < 1) { + // bad dt, can't divide by it + return; + } + + dt /= SEC2USEC; + + if (!_last_slewrate_time) { + // running the first time since switching to precland + + // assume dt will be about 50000us + dt = 50000 / SEC2USEC; + + // set a best guess for previous setpoints for smooth transition + map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat, + _navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1)); + _sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt; + _sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt; + } + + _last_slewrate_time = now; + + // limit the setpoint speed to the maximum cruise speed + matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints + + if (sp_vel.length() > _param_xy_vel_cruise) { + sp_vel = sp_vel.normalized() * _param_xy_vel_cruise; + sp_curr = _sp_pev + sp_vel * dt; + } + + // limit the setpoint acceleration to the maximum acceleration + matrix::Vector2f sp_acc = (sp_curr - _sp_pev * 2 + _sp_pev_prev) / (dt * dt); // acceleration of the setpoints + + if (sp_acc.length() > _param_acceleration_hor) { + sp_acc = sp_acc.normalized() * _param_acceleration_hor; + sp_curr = _sp_pev * 2 - _sp_pev_prev + sp_acc * (dt * dt); + } + + // limit the setpoint speed such that we can stop at the setpoint given the maximum acceleration/deceleration + float max_spd = sqrtf(_param_acceleration_hor * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x, + sp_y))).length()); + sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints + + if (sp_vel.length() > max_spd) { + sp_vel = sp_vel.normalized() * max_spd; + sp_curr = _sp_pev + sp_vel * dt; + } + + _sp_pev_prev = _sp_pev; + _sp_pev = sp_curr; + + sp_x = sp_curr(0); + sp_y = sp_curr(1); +} diff --git a/src/prometheus_px4/src/modules/navigator/precland.h b/src/prometheus_px4/src/modules/navigator/precland.h new file mode 100644 index 0000000..1246e30 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/precland.h @@ -0,0 +1,148 @@ +/*************************************************************************** + * + * Copyright (c) 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 precland.h + * + * Helper class to do precision landing with a landing target + * + * @author Nicolas de Palezieux (Sunflower Labs) + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +enum class PrecLandState { + Start, // Starting state + HorizontalApproach, // Positioning over landing target while maintaining altitude + DescendAboveTarget, // Stay over landing target while descending + FinalApproach, // Final landing approach, even without landing target + Search, // Search for landing target + Fallback, // Fallback landing method + Done // Done landing +}; + +enum class PrecLandMode { + Opportunistic = 1, // only do precision landing if landing target visible at the beginning + Required = 2 // try to find landing target if not visible at the beginning +}; + +class PrecLand : public MissionBlock, public ModuleParams +{ +public: + PrecLand(Navigator *navigator); + ~PrecLand() override = default; + + void on_activation() override; + void on_active() override; + void on_inactivation() override; + + void set_mode(PrecLandMode mode) { _mode = mode; }; + + PrecLandMode get_mode() { return _mode; }; + + bool is_activated() { return _is_activated; }; + +private: + + void updateParams() override; + + // run the control loop for each state + void run_state_start(); + void run_state_horizontal_approach(); + void run_state_descend_above_target(); + void run_state_final_approach(); + void run_state_search(); + void run_state_fallback(); + + // attempt to switch to a different state. Returns true if state change was successful, false otherwise + bool switch_to_state_start(); + bool switch_to_state_horizontal_approach(); + bool switch_to_state_descend_above_target(); + bool switch_to_state_final_approach(); + bool switch_to_state_search(); + bool switch_to_state_fallback(); + bool switch_to_state_done(); + + // check if a given state could be changed into. Return true if possible to transition to state, false otherwise + bool check_state_conditions(PrecLandState state); + void slewrate(float &sp_x, float &sp_y); + + landing_target_pose_s _target_pose{}; /**< precision landing target position */ + + uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)}; + bool _target_pose_valid{false}; /**< whether we have received a landing target position message */ + bool _target_pose_updated{false}; /**< wether the landing target position message is updated */ + + struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */ + + uint64_t _state_start_time{0}; /**< time when we entered current state */ + uint64_t _last_slewrate_time{0}; /**< time when we last limited setpoint changes */ + uint64_t _target_acquired_time{0}; /**< time when we first saw the landing target during search */ + uint64_t _point_reached_time{0}; /**< time when we reached a setpoint */ + + int _search_cnt{0}; /**< counter of how many times we had to search for the landing target */ + float _approach_alt{0.0f}; /**< altitude at which to stay during horizontal approach */ + + matrix::Vector2f _sp_pev; + matrix::Vector2f _sp_pev_prev; + + PrecLandState _state{PrecLandState::Start}; + + PrecLandMode _mode{PrecLandMode::Opportunistic}; + + bool _is_activated {false}; /**< indicates if precland is activated */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_pld_btout, + (ParamFloat) _param_pld_hacc_rad, + (ParamFloat) _param_pld_fappr_alt, + (ParamFloat) _param_pld_srch_alt, + (ParamFloat) _param_pld_srch_tout, + (ParamInt) _param_pld_max_srch + ) + + // non-navigator parameters + param_t _handle_param_acceleration_hor{PARAM_INVALID}; + param_t _handle_param_xy_vel_cruise{PARAM_INVALID}; + float _param_acceleration_hor{0.0f}; + float _param_xy_vel_cruise{0.0f}; + +}; diff --git a/src/prometheus_px4/src/modules/navigator/precland_params.c b/src/prometheus_px4/src/modules/navigator/precland_params.c new file mode 100644 index 0000000..efd130c --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/precland_params.c @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 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 precland_params.c + * + * Parameters for precision landing. + * + * @author Nicolas de Palezieux (Sunflower Labs) + */ + +/** + * Landing Target Timeout + * + * Time after which the landing target is considered lost without any new measurements. + * + * @unit s + * @min 0.0 + * @max 50 + * @decimal 1 + * @increment 0.5 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_BTOUT, 5.0f); + +/** + * Horizontal acceptance radius + * + * Start descending if closer above landing target than this. + * + * @unit m + * @min 0.0 + * @max 10 + * @decimal 2 + * @increment 0.1 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_HACC_RAD, 0.2f); + +/** + * Final approach altitude + * + * Allow final approach (without horizontal positioning) if losing landing target closer than this to the ground. + * + * @unit m + * @min 0.0 + * @max 10 + * @decimal 2 + * @increment 0.1 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_FAPPR_ALT, 0.1f); + +/** + * Search altitude + * + * Altitude above home to which to climb when searching for the landing target. + * + * @unit m + * @min 0.0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_SRCH_ALT, 10.0f); + +/** + * Search timeout + * + * Time allowed to search for the landing target before falling back to normal landing. + * + * @unit s + * @min 0.0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group Precision Land + */ +PARAM_DEFINE_FLOAT(PLD_SRCH_TOUT, 10.0f); + +/** + * Maximum number of search attempts + * + * Maximum number of times to seach for the landing target if it is lost during the precision landing. + * + * @min 0 + * @max 100 + * @group Precision Land + */ +PARAM_DEFINE_INT32(PLD_MAX_SRCH, 3); diff --git a/src/prometheus_px4/src/modules/navigator/rtl.cpp b/src/prometheus_px4/src/modules/navigator/rtl.cpp new file mode 100644 index 0000000..f10dd8f --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/rtl.cpp @@ -0,0 +1,737 @@ +/**************************************************************************** + * + * 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 rtl.cpp + * + * Helper class to access RTL + * + * @author Julian Oes + * @author Anton Babushkin + * @author Julian Kent + */ + +#include "rtl.h" +#include "navigator.h" +#include + +#include + + +static constexpr float DELAY_SIGMA = 0.01f; + +using namespace time_literals; +using namespace math; + +RTL::RTL(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ +} + +void RTL::on_inactivation() +{ + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void RTL::on_inactive() +{ + // Reset RTL state. + _rtl_state = RTL_STATE_NONE; + + find_RTL_destination(); +} + +void RTL::find_RTL_destination() +{ + // don't update RTL destination faster than 1 Hz + if (hrt_elapsed_time(&_destination_check_time) < 1_s) { + return; + } + + if (!_navigator->home_position_valid()) { + return; + } + + _destination_check_time = hrt_absolute_time(); + + // get home position: + home_position_s &home_landing_position = *_navigator->get_home_position(); + + // get global position + const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + + // set destination to home per default, then check if other valid landing spot is closer + _destination.set(home_landing_position); + + // get distance to home position + double dlat = home_landing_position.lat - global_position.lat; + double dlon = home_landing_position.lon - global_position.lon; + + double lon_scale = cos(radians(global_position.lat)); + + auto coord_dist_sq = [lon_scale](double lat_diff, double lon_diff) -> double { + double lon_diff_scaled = lon_scale * matrix::wrap(lon_diff, -180., 180.); + return lat_diff * lat_diff + lon_diff_scaled * lon_diff_scaled; + }; + + double min_dist_squared = coord_dist_sq(dlat, dlon); + + _destination.type = RTL_DESTINATION_HOME; + + const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + + // consider the mission landing if not RTL_HOME type set + if (rtl_type() != RTL_HOME && _navigator->get_mission_start_land_available()) { + double mission_landing_lat; + double mission_landing_lon; + float mission_landing_alt; + RTLDestinationType destination_type = RTL_DESTINATION_MISSION_LANDING; + + if (vtol_in_rw_mode) { + mission_landing_lat = _navigator->get_mission_landing_lat(); + mission_landing_lon = _navigator->get_mission_landing_lon(); + mission_landing_alt = _navigator->get_mission_landing_alt(); + destination_type = RTL_DESTINATION_HOME; + + } else { + mission_landing_lat = _navigator->get_mission_landing_start_lat(); + mission_landing_lon = _navigator->get_mission_landing_start_lon(); + mission_landing_alt = _navigator->get_mission_landing_start_alt(); + } + + // compare home position to landing position to decide which is closer + dlat = mission_landing_lat - global_position.lat; + dlon = mission_landing_lon - global_position.lon; + double dist_squared = coord_dist_sq(dlat, dlon); + + // set destination to mission landing if closest or in RTL_LAND or RTL_MISSION (so not in RTL_CLOSEST) + if (dist_squared < min_dist_squared || rtl_type() != RTL_CLOSEST) { + min_dist_squared = dist_squared; + _destination.lat = mission_landing_lat; + _destination.lon = mission_landing_lon; + _destination.alt = mission_landing_alt; + _destination.type = destination_type; + + + } + } + + // do not consider rally point if RTL type is set to RTL_MISSION, so exit function and use either home or mission landing + if (rtl_type() == RTL_MISSION) { + return; + } + + // compare to safe landing positions + mission_safe_point_s closest_safe_point {}; + mission_stats_entry_s stats; + int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + int num_safe_points = 0; + + if (ret == sizeof(mission_stats_entry_s)) { + num_safe_points = stats.num_items; + } + + // check if a safe point is closer than home or landing + int closest_index = 0; + + for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { + mission_safe_point_s mission_safe_point; + + if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) != + sizeof(mission_safe_point_s)) { + PX4_ERR("dm_read failed"); + continue; + } + + // TODO: take altitude into account for distance measurement + dlat = mission_safe_point.lat - global_position.lat; + dlon = mission_safe_point.lon - global_position.lon; + double dist_squared = coord_dist_sq(dlat, dlon); + + if (dist_squared < min_dist_squared) { + closest_index = current_seq; + min_dist_squared = dist_squared; + closest_safe_point = mission_safe_point; + } + } + + if (closest_index > 0) { + _destination.type = RTL_DESTINATION_SAFE_POINT; + + // There is a safe point closer than home/mission landing + // TODO: handle all possible mission_safe_point.frame cases + switch (closest_safe_point.frame) { + case 0: // MAV_FRAME_GLOBAL + _destination.lat = closest_safe_point.lat; + _destination.lon = closest_safe_point.lon; + _destination.alt = closest_safe_point.alt; + _destination.yaw = home_landing_position.yaw; + break; + + case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT + _destination.lat = closest_safe_point.lat; + _destination.lon = closest_safe_point.lon; + _destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home + _destination.yaw = home_landing_position.yaw; + break; + + default: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME"); + break; + } + } + + // figure out how long the RTL will take + float rtl_xy_speed, rtl_z_speed; + get_rtl_xy_z_speed(rtl_xy_speed, rtl_z_speed); + + matrix::Vector3f to_destination_vec; + get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon, + &to_destination_vec(0), &to_destination_vec(1)); + to_destination_vec(2) = _destination.alt - global_position.alt; + + float time_to_home_s = time_to_home(to_destination_vec, get_wind(), rtl_xy_speed, rtl_z_speed); + + float rtl_flight_time_ratio = time_to_home_s / (60 * _param_rtl_flt_time.get()); + rtl_flight_time_s rtl_flight_time{}; + rtl_flight_time.timestamp = hrt_absolute_time(); + rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio; + rtl_flight_time.rtl_time_s = time_to_home_s; + _rtl_flight_time_pub.publish(rtl_flight_time); +} + +void RTL::on_activation() +{ + + _deny_mission_landing = _navigator->get_vstatus()->is_vtol + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + // output the correct message, depending on where the RTL destination is + switch (_destination.type) { + case RTL_DESTINATION_HOME: + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position."); + break; + + case RTL_DESTINATION_MISSION_LANDING: + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing."); + break; + + case RTL_DESTINATION_SAFE_POINT: + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point."); + break; + } + + const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + + _rtl_loiter_rad = _param_rtl_loiter_rad.get(); + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); + + } else { + _rtl_alt = max(global_position.alt, max(_destination.alt, + _navigator->get_home_position()->alt + _param_rtl_return_alt.get())); + } + + + if (_navigator->get_land_detected()->landed) { + // For safety reasons don't go into RTL if landed. + _rtl_state = RTL_STATE_LANDED; + + } else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) { + // we were just on a mission landing, set _rtl_state past RTL_STATE_RETURN such that navigator will engage mission mode, + // which will continue executing the landing + _rtl_state = RTL_STATE_DESCEND; + + + } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { + + // If lower than return altitude, climb up first. + // If rtl_alt_min is true then forcing altitude change even if above. + _rtl_state = RTL_STATE_CLIMB; + + } else { + // Otherwise go straight to return + _rtl_state = RTL_STATE_RETURN; + } + + setClimbAndReturnDone(_rtl_state > RTL_STATE_RETURN); + + set_rtl_item(); + +} + +void RTL::on_active() +{ + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(); + } + + if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void RTL::set_rtl_item() +{ + // RTL_TYPE: mission landing. + // Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination. + // After reaching DO_LAND_START, do nothing, let navigator takeover with mission landing. + if (_destination.type == RTL_DESTINATION_MISSION_LANDING) { + if (_rtl_state > RTL_STATE_RETURN) { + if (_navigator->start_mission_landing()) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing"); + return; + + } else { + // Otherwise use regular RTL. + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing"); + } + } + } + + _navigator->set_can_loiter_at_sp(false); + + const vehicle_global_position_s &gpos = *_navigator->get_global_position(); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); + const float descend_altitude_target = min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt); + const float loiter_altitude = min(descend_altitude_target, _rtl_alt); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + // do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT, + // even if current climb altitude is below (e.g. RTL immediately after take off) + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + } + + _mission_item.lat = gpos.lat; + _mission_item.lon = gpos.lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)", + (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); + break; + } + + case RTL_STATE_RETURN: { + // Don't change altitude. + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + + // Use destination yaw if close to _destination. + // Check if we are pretty close to the destination already. + if (destination_dist < _param_rtl_min_dist.get()) { + _mission_item.yaw = _destination.yaw; + + } else { + // Use current heading to _destination. + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + // Except for vtol which might be still off here and should point towards this location. + const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); + + if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) { + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); + + } else { + _mission_item.yaw = _destination.yaw; + } + + if (_navigator->get_vstatus()->is_vtol) { + _mission_item.loiter_radius = _rtl_loiter_rad; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + break; + } + + case RTL_STATE_LOITER: { + const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON); + + // Don't change altitude. + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = _destination.yaw; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + + if (autoland) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", + (double)get_time_inside(_mission_item)); + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering"); + } + + break; + } + + case RTL_STATE_HEAD_TO_CENTER: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + break; + } + + case RTL_STATE_TRANSITION_TO_MC: { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + break; + } + + case RTL_MOVE_TO_LAND_HOVER_VTOL: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.origin = ORIGIN_ONBOARD; + break; + } + + case RTL_STATE_LAND: { + // Land at destination. + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.yaw = _destination.yaw; + _mission_item.altitude = _destination.alt; + _mission_item.altitude_is_relative = false; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.land_precision = _param_rtl_pld_md.get(); + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + _navigator->get_precland()->on_activation(); + + } else if (_mission_item.land_precision == 2) { + _navigator->get_precland()->set_mode(PrecLandMode::Required); + _navigator->get_precland()->on_activation(); + } + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination"); + break; + } + + case RTL_STATE_LANDED: { + set_idle_item(&_mission_item); + set_return_alt_min(false); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + // Execute command if set. This is required for commands like VTOL transition. + if (!item_contains_position(_mission_item)) { + issue_command(_mission_item); + } + + // Convert mission item to current position setpoint and make it valid. + mission_apply_limitation(_mission_item); + + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void RTL::advance_rtl() +{ + // determines if the vehicle should loiter above land + const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; + + // vehicle is a vtol and currently in fixed wing mode + const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + setClimbAndReturnDone(true); + + if (vtol_in_fw_mode || descend_and_loiter) { + _rtl_state = RTL_STATE_DESCEND; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_DESCEND: + + if (descend_and_loiter) { + _rtl_state = RTL_STATE_LOITER; + + } else if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_HEAD_TO_CENTER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_LOITER: + if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_TRANSITION_TO_MC; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + _rtl_state = RTL_STATE_LAND; + break; + + case RTL_STATE_HEAD_TO_CENTER: + + _rtl_state = RTL_STATE_TRANSITION_TO_MC; + + break; + + case RTL_STATE_TRANSITION_TO_MC: + + _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; + + break; + + case RTL_MOVE_TO_LAND_HOVER_VTOL: + + _rtl_state = RTL_STATE_LAND; + + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + break; + + default: + break; + } +} + +float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) +{ + const vehicle_global_position_s &gpos = *_navigator->get_global_position(); + + // horizontal distance to destination + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); + + // minium rtl altitude to use when outside of horizontal acceptance radius of target position. + // We choose the minimum height to be two times the distance from the land position in order to + // avoid the vehicle touching the ground while still moving horizontally. + const float return_altitude_min_outside_acceptance_rad_amsl = _destination.alt + 2.0f * + _navigator->get_acceptance_radius(); + + float return_altitude_amsl = _destination.alt + _param_rtl_return_alt.get(); + + if (destination_dist <= _navigator->get_acceptance_radius()) { + return_altitude_amsl = _destination.alt + 2.0f * destination_dist; + + } else { + + if (cone_half_angle_deg > 0.0f && destination_dist <= _param_rtl_min_dist.get()) { + + // constrain cone half angle to meaningful values. All other cases are already handled above. + const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f)); + + // minimum altitude we need in order to be within the user defined cone + const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + _destination.alt; + + return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl); + } + + return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); + } + + return max(return_altitude_amsl, gpos.alt); +} + +void RTL::get_rtl_xy_z_speed(float &xy, float &z) +{ + uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type; + // Caution: here be dragons! + // Use C API to allow this code to be compiled with builds that don't have FW/MC/Rover + + if (vehicle_type != _rtl_vehicle_type) { + _rtl_vehicle_type = vehicle_type; + + switch (vehicle_type) { + case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: + _param_rtl_xy_speed = param_find("MPC_XY_CRUISE"); + _param_rtl_descent_speed = param_find("MPC_Z_VEL_MAX_DN"); + break; + + case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: + _param_rtl_xy_speed = param_find("FW_AIRSPD_TRIM"); + _param_rtl_descent_speed = param_find("FW_T_SINK_MIN"); + break; + + case vehicle_status_s::VEHICLE_TYPE_ROVER: + _param_rtl_xy_speed = param_find("GND_SPEED_THR_SC"); + _param_rtl_descent_speed = PARAM_INVALID; + break; + } + } + + if ((_param_rtl_xy_speed == PARAM_INVALID) || param_get(_param_rtl_xy_speed, &xy) != PX4_OK) { + xy = 1e6f; + } + + if ((_param_rtl_descent_speed == PARAM_INVALID) || param_get(_param_rtl_descent_speed, &z) != PX4_OK) { + z = 1e6f; + } + +} + +matrix::Vector2f RTL::get_wind() +{ + _wind_sub.update(); + matrix::Vector2f wind; + + if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { + wind(0) = _wind_sub.get().windspeed_north; + wind(1) = _wind_sub.get().windspeed_east; + } + + return wind; +} + +float time_to_home(const matrix::Vector3f &to_home_vec, + const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, float vehicle_descent_speed_m_s) +{ + const matrix::Vector2f to_home = to_home_vec.xy(); + const float alt_change = to_home_vec(2); + const matrix::Vector2f to_home_dir = to_home.unit_or_zero(); + const float dist_to_home = to_home.norm(); + + const float wind_towards_home = wind_velocity.dot(to_home_dir); + const float wind_across_home = matrix::Vector2f(wind_velocity - to_home_dir * wind_towards_home).norm(); + + // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient + const float cruise_speed = sqrtf(vehicle_speed_m_s * vehicle_speed_m_s - wind_across_home * wind_across_home) + fminf( + 0.f, wind_towards_home); + + if (!PX4_ISFINITE(cruise_speed) || cruise_speed <= 0) { + return INFINITY; // we never reach home if the wind is stronger than vehicle speed + } + + // assume horizontal and vertical motions happen serially, so their time adds + float horiz = dist_to_home / cruise_speed; + float descent = fabsf(alt_change) / vehicle_descent_speed_m_s; + return horiz + descent; +} diff --git a/src/prometheus_px4/src/modules/navigator/rtl.h b/src/prometheus_px4/src/modules/navigator/rtl.h new file mode 100644 index 0000000..d7cb010 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/rtl.h @@ -0,0 +1,181 @@ +/*************************************************************************** + * + * 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 rtl.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +#include +#include +#include +#include +#include +#include +#include + +class Navigator; + +class RTL : public MissionBlock, public ModuleParams +{ +public: + enum RTLType { + RTL_HOME = 0, + RTL_LAND, + RTL_MISSION, + RTL_CLOSEST, + }; + + enum RTLDestinationType { + RTL_DESTINATION_HOME = 0, + RTL_DESTINATION_MISSION_LANDING, + RTL_DESTINATION_SAFE_POINT, + }; + + RTL(Navigator *navigator); + + ~RTL() = default; + + void on_inactivation() override; + void on_inactive() override; + void on_activation() override; + void on_active() override; + + void find_RTL_destination(); + + void set_return_alt_min(bool min) { _rtl_alt_min = min; } + + int rtl_type() const { return _param_rtl_type.get(); } + + int rtl_destination(); + + void setClimbAndReturnDone(bool done) { _climb_and_return_done = done; } + + bool getClimbAndReturnDone() { return _climb_and_return_done; } + + bool denyMissionLanding() { return _deny_mission_landing; } + + void get_rtl_xy_z_speed(float &xy, float &z); + matrix::Vector2f get_wind(); +private: + /** + * Set the RTL item + */ + void set_rtl_item(); + + /** + * Move to next RTL item + */ + void advance_rtl(); + + + float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_TRANSITION_TO_MC, + RTL_MOVE_TO_LAND_HOVER_VTOL, + RTL_STATE_LAND, + RTL_STATE_LANDED, + RTL_STATE_HEAD_TO_CENTER, + } _rtl_state{RTL_STATE_NONE}; + + struct RTLPosition { + double lat; + double lon; + float alt; + float yaw; + uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points) + RTLDestinationType type{RTL_DESTINATION_HOME}; + + void set(const home_position_s &home_position) + { + lat = home_position.lat; + lon = home_position.lon; + alt = home_position.alt; + yaw = home_position.yaw; + safe_point_index = 0; + type = RTL_DESTINATION_HOME; + } + }; + + RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) + + hrt_abstime _destination_check_time{0}; + + float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position + bool _rtl_alt_min{false}; + float _rtl_loiter_rad{50.0f}; // radius at which a fixed wing would loiter while descending + bool _climb_and_return_done{false}; // this flag is set to true if RTL is active and we are past the climb state and return state + bool _deny_mission_landing{false}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_descend_alt, + (ParamFloat) _param_rtl_land_delay, + (ParamFloat) _param_rtl_min_dist, + (ParamInt) _param_rtl_type, + (ParamInt) _param_rtl_cone_half_angle_deg, + (ParamFloat) _param_rtl_flt_time, + (ParamInt) _param_rtl_pld_md, + (ParamFloat) _param_rtl_loiter_rad + ) + + // These need to point at different parameters depending on vehicle type. + // Can't hard-code them because we have non-MC/FW/Rover builds + uint8_t _rtl_vehicle_type{vehicle_status_s::VEHICLE_TYPE_UNKNOWN}; + + param_t _param_rtl_xy_speed{PARAM_INVALID}; + param_t _param_rtl_descent_speed{PARAM_INVALID}; + + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; + uORB::Publication _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; +}; + +float time_to_home(const matrix::Vector3f &to_home_vec, + const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, + float vehicle_descent_speed_m_s); diff --git a/src/prometheus_px4/src/modules/navigator/rtl_params.c b/src/prometheus_px4/src/modules/navigator/rtl_params.c new file mode 100644 index 0000000..463ee1a --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/rtl_params.c @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 rtl_params.c + * + * Parameters for return mode + * + * @author Julian Oes + */ + +/* + * Return mode parameters, accessible via MAVLink + */ + +/** + * Return mode return altitude + * + * Default minimum altitude above destination (e.g. home, safe point, landing pattern) for return flight. + * This is affected by RTL_MIN_DIST and RTL_CONE_ANG. + * + * @unit m + * @min 0 + * @max 150 + * @decimal 1 + * @increment 0.5 + * @group Return Mode + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); + + +/** + * Return mode loiter altitude + * + * Descend to this altitude (above destination position) after return, and wait for time defined in RTL_LAND_DELAY. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit m + * @min 2 + * @max 100 + * @decimal 1 + * @increment 0.5 + * @group Return Mode + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30); + +/** + * Return mode delay + * + * Delay before landing (after initial descent) in Return mode. + * If set to -1 the system will not land but loiter at RTL_DESCEND_ALT. + * + * @unit s + * @min -1 + * @max 300 + * @decimal 1 + * @increment 0.5 + * @group Return Mode + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, 0.0f); + +/** + * Horizontal radius from return point within which special rules for return mode apply. + * + * The return altitude will be calculated based on RTL_CONE_ANG parameter. + * The yaw setpoint will switch to the one defined by corresponding waypoint. + * + * + * @unit m + * @min 0.5 + * @max 100 + * @decimal 1 + * @increment 0.5 + * @group Return Mode + */ +PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f); + +/** + * Return type + * + * Return mode destination and flight path (home location, rally point, mission landing pattern, reverse mission) + * + * @value 0 Return to closest safe point (home or rally point) via direct path. + * @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path. + * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points. + * @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land. + * @group Return Mode + */ +PARAM_DEFINE_INT32(RTL_TYPE, 0); + +/** + * Half-angle of the return mode altitude cone + * + * Defines the half-angle of a cone centered around the destination position that + * affects the altitude at which the vehicle returns. + * + * @unit deg + * @min 0 + * @max 90 + * @value 0 No cone, always climb to RTL_RETURN_ALT above destination. + * @value 25 25 degrees half cone angle. + * @value 45 45 degrees half cone angle. + * @value 65 65 degrees half cone angle. + * @value 80 80 degrees half cone angle. + * @value 90 Only climb to at least RTL_DESCEND_ALT above destination. + * @group Return Mode + */ +PARAM_DEFINE_INT32(RTL_CONE_ANG, 45); + +/** + * Maximum allowed RTL flight in minutes + * + * This is used to determine when the vehicle should be switched to RTL due to low battery. + * Note, particularly for multirotors this should reflect flight time at cruise speed, not while stationary + * + * @unit min + * @group Commander + */ +PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15); + +/** + * RTL precision land mode + * + * Use precision landing when doing an RTL landing phase. + * + * @value 0 No precision landing + * @value 1 Opportunistic precision landing + * @value 2 Required precision landing + * @group Return To Land + */ +PARAM_DEFINE_INT32(RTL_PLD_MD, 0); + +/** + * Loiter radius for rtl descend + * + * Set the radius for loitering to a safe altitude for VTOL transition. + * + * @unit m + * @min 25 + * @max 1000 + * @decimal 1 + * @increment 0.5 + * @group Return Mode + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); diff --git a/src/prometheus_px4/src/modules/navigator/takeoff.cpp b/src/prometheus_px4/src/modules/navigator/takeoff.cpp new file mode 100644 index 0000000..2bde007 --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/takeoff.cpp @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * 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 takeoff.cpp + * + * Helper class to Takeoff + * + * @author Lorenz Meier + */ + +#include "takeoff.h" +#include "navigator.h" + +Takeoff::Takeoff(Navigator *navigator) : + MissionBlock(navigator) +{ +} + +void +Takeoff::on_activation() +{ + set_takeoff_position(); +} + +void +Takeoff::on_active() +{ + struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); + + if (rep->current.valid) { + // reset the position + set_takeoff_position(); + + } else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + + // set loiter item so position controllers stop doing takeoff logic + set_loiter_item(&_mission_item); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void +Takeoff::set_takeoff_position() +{ + struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); + + float abs_altitude = 0.0f; + + float min_abs_altitude; + + if (_navigator->home_position_valid()) { //only use home position if it is valid + min_abs_altitude = _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt(); + + } else { //e.g. flow + min_abs_altitude = _navigator->get_takeoff_min_alt(); + } + + // Use altitude if it has been set. If home position is invalid use min_abs_altitude + if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) { + abs_altitude = rep->current.alt; + + // If the altitude suggestion is lower than home + minimum clearance, raise it and complain. + if (abs_altitude < min_abs_altitude) { + if (abs_altitude < min_abs_altitude - 0.1f) { // don't complain if difference is smaller than 10cm + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt()); + } + + abs_altitude = min_abs_altitude; + } + + } else { + // Use home + minimum clearance but only notify. + abs_altitude = min_abs_altitude; + mavlink_log_info(_navigator->get_mavlink_log_pub(), + "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt()); + } + + + if (abs_altitude < _navigator->get_global_position()->alt) { + // If the suggestion is lower than our current alt, let's not go down. + abs_altitude = _navigator->get_global_position()->alt; + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude"); + } + + // set current mission item to takeoff + set_takeoff_item(&_mission_item, abs_altitude); + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); + + // convert mission item to current setpoint + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.yaw_valid = true; + pos_sp_triplet->next.valid = false; + + if (rep->current.valid) { + + // Go on and check which changes had been requested + if (PX4_ISFINITE(rep->current.yaw)) { + pos_sp_triplet->current.yaw = rep->current.yaw; + } + + // Set the current latitude and longitude even if they are NAN + // NANs are handled in FlightTaskAuto.cpp + pos_sp_triplet->current.lat = rep->current.lat; + pos_sp_triplet->current.lon = rep->current.lon; + + // mark this as done + memset(rep, 0, sizeof(*rep)); + } + + if (PX4_ISFINITE(pos_sp_triplet->current.lat) && PX4_ISFINITE(pos_sp_triplet->current.lon)) { + _navigator->set_can_loiter_at_sp(true); + + } else { + _navigator->set_can_loiter_at_sp(false); + } + + _navigator->set_position_setpoint_triplet_updated(); +} diff --git a/src/prometheus_px4/src/modules/navigator/takeoff.h b/src/prometheus_px4/src/modules/navigator/takeoff.h new file mode 100644 index 0000000..6fb5d8e --- /dev/null +++ b/src/prometheus_px4/src/modules/navigator/takeoff.h @@ -0,0 +1,58 @@ +/*************************************************************************** + * + * Copyright (c) 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 takeoff.h + * + * Helper class to take off + * + * @author Lorenz Meier + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" + +class Takeoff : public MissionBlock +{ +public: + Takeoff(Navigator *navigator); + ~Takeoff() = default; + + void on_activation() override; + void on_active() override; + +private: + + void set_takeoff_position(); +}; diff --git a/src/prometheus_px4/src/modules/offline_task_manager/CMakeLists.txt b/src/prometheus_px4/src/modules/offline_task_manager/CMakeLists.txt new file mode 100644 index 0000000..9680f66 --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2024 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__offline_task_manager + MAIN offline_task_manager + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + -Wno-double-promotion + -Wno-float-equal + SRCS + offline_task_manager.cpp + offline_task_parser.cpp + offline_task_planner.cpp + cJSON.c + DEPENDS + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/offline_task_manager/README.md b/src/prometheus_px4/src/modules/offline_task_manager/README.md new file mode 100644 index 0000000..43c6c43 --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/README.md @@ -0,0 +1,154 @@ +# Offline Task Manager 离线任务管理模块 + +## 简介 + +`offline_task_manager` 是 PX4 自动驾驶系统的一个内部模块，用于实现无人机的离线任务托管和自主航线执行。该模块从本地 JSON 文件读取预规划任务点，根据优先级和电池状态进行路径规划，自动生成 PX4 mission 并触发执行。 + +## 功能特性 + +- **JSON 任务文件读取**：支持从 `/tmp/offline_tasks.json` 读取离线任务定义 +- **智能路径规划**：基于最近邻算法（Nearest Neighbor）优化任务执行顺序 +- **任务优先级支持**：支持紧急任务（must_first/priority=5）优先执行 +- **电池感知**：根据当前电量和预估消耗过滤不可达任务 +- **自动模式切换**：自动解锁无人机并切换至 `AUTO_MISSION` 模式 +- **完整状态机**：从空闲到任务监控的全流程状态管理 + +## 使用流程 + +### 1. 准备任务文件 + +创建 `/tmp/offline_tasks.json`： + +```json +{ + "version": 1, + "mission_id": "test_mission", + "start_mode": "auto", + "tasks": [ + { + "task_id": 1, + "latitude": 47.397800, + "longitude": 8.545600, + "altitude": 520.0, + "priority": 5, + "must_first": false + } + ] +} +``` + +### 2. 启动模块 + +在 PX4 Shell 中执行： + +```bash +# 启动模块 +offline_task_manager start + +# 查看状态 +offline_task_manager status +``` + +### 3. 触发任务执行 + +```bash +# 启用模块 +param set OTM_ENABLE 1 + +# 开始任务 +param set OTM_START 1 +``` + +模块将自动执行： +1. 等待 GPS 定位有效 +2. 读取 JSON 任务文件 +3. 路径规划（最近邻算法） +4. 写入 mission 到 dataman +5. 解锁并切换至 AUTO_MISSION 模式 +6. 监控任务执行 + +## 模块参数 + +| 参数名 | 默认值 | 说明 | +|--------|--------|------| +| `OTM_ENABLE` | 0 | 启用/禁用模块 (0=禁用, 1=启用) | +| `OTM_START` | 0 | 触发任务执行 (设为1开始) | +| `OTM_LOW_BATT` | 20.0 | 低电量阈值 (%) | +| `OTM_BATT_INIT` | 100.0 | 初始模拟电量 (%) | +| `OTM_BATT_DRAIN` | 0.5 | 模拟电量消耗率 (%/s) | +| `OTM_FILE_PATH` | /tmp/offline_tasks.json | 任务文件路径 | +| `OTM_HOLD_T` | 10.0 | 航点悬停时间 (秒) | +| `OTM_ACC_RAD` | 2.0 | 航点接受半径 (米) | + +## 文件结构 + +``` +src/modules/offline_task_manager/ +├── CMakeLists.txt # 构建配置 +├── README.md # 本文件 +├── offline_task.hpp # 任务数据结构 +├── cJSON.h / cJSON.c # JSON 解析库 +├── offline_task_parser.hpp/cpp # 任务文件解析 +├── offline_task_planner.hpp/cpp # 路径规划器 +├── offline_task_manager_params.c # PX4 参数定义 +├── offline_task_manager.hpp/cpp # 主模块实现 +``` + +## 状态机 + +``` +IDLE (空闲) + ↓ +WAIT_FOR_HOME (等待设置home点) + ↓ +WAIT_FOR_POSITION (等待GPS定位) + ↓ +LOAD_TASK_FILE (加载JSON任务文件) + ↓ +VALIDATE_TASKS (验证任务有效性) + ↓ +PLAN_TASKS (路径规划) + ↓ +WRITE_MISSION (写入mission到dataman) + ↓ +START_MISSION (解锁并启动任务) + ↓ +MONITOR_MISSION (监控执行) + ↓ +IDLE (完成，返回空闲) +``` + +## 集成说明 + +- **构建目标**：已添加到 `amovlab_sitl_default` 和 `px4_sitl_default` +- **uORB 主题订阅**： + - `battery_status` - 电池状态 + - `vehicle_global_position` - 全局位置 + - `home_position` - Home点位置 +- **uORB 发布**： + - `mission` - Mission状态 + - `vehicle_command` - 飞行模式切换命令 +- **存储**：使用 dataman 持久化存储 mission items + +## SITL 测试 + +```bash +# 启动仿真 +make amovlab_sitl_default gazebo_p450 + +# 在 PX4 Shell 中 +offline_task_manager start +param set OTM_ENABLE 1 +param set OTM_START 1 +``` + +**注意**：为避免数据链丢失保护触发返航，建议在 QGroundControl 中设置参数： +- `NAV_DLL_ACT = 0` (禁用数据链丢失保护) +- `COM_RC_IN_MODE = 1` (虚拟摇杆模式) + +## 限制说明 + +- 最大支持 32 个离线任务点 +- 当前仅支持多旋翼飞行器 (multicopter) +- 任务点高度为绝对海拔高度 (AMSL) +- SITL 测试时需要配合地面站或禁用相关 failsafe \ No newline at end of file diff --git a/src/prometheus_px4/src/modules/offline_task_manager/cJSON.c b/src/prometheus_px4/src/modules/offline_task_manager/cJSON.c new file mode 100644 index 0000000..9d0db1d --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/cJSON.c @@ -0,0 +1,416 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +/* cJSON minimal implementation for PX4 offline_task_manager */ + +#include "cJSON.h" +#include +#include +#include +#include +#include + +/* Parse text to find a number */ +static const char *parse_number(cJSON *item, const char *num) +{ + double n = 0; + double scale = 0; + int subscale = 0; + int signsubscale = 1; + int sign = 1; + + if (*num == '-') { + sign = -1; + num++; + } + + if (*num == '0') { + num++; + } + + if ((*num >= '1') && (*num <= '9')) { + do { + n = (n * 10.0) + (*num++ - '0'); + } while ((*num >= '0') && (*num <= '9')); + } + + if ((*num == '.') && (num[1] >= '0') && (num[1] <= '9')) { + num++; + do { + n = (n * 10.0) + (*num++ - '0'); + scale--; + } while ((*num >= '0') && (*num <= '9')); + } + + if ((*num == 'e') || (*num == 'E')) { + num++; + if (*num == '+') { + num++; + } else if (*num == '-') { + signsubscale = -1; + num++; + } + + while ((*num >= '0') && (*num <= '9')) { + subscale = (subscale * 10) + (*num++ - '0'); + } + } + + n = sign * n * pow(10.0, (scale + (subscale * signsubscale))); + + item->valuedouble = n; + item->valueint = (int)n; + item->type = cJSON_Number; + + return num; +} + +/* Parse string */ +static const char *parse_string(cJSON *item, const char *str) +{ + const char *ptr = str + 1; + char *ptr2; + char *out; + int len = 0; + + if (*str != '\"') { + return NULL; + } + + while ((*ptr != '\"') && *ptr) { + if (*ptr++ == '\\') { + ptr++; + } + len++; + } + + out = (char *)malloc(len + 1); + if (!out) { + return NULL; + } + + ptr = str + 1; + ptr2 = out; + + while ((*ptr != '\"') && *ptr) { + if (*ptr != '\\') { + *ptr2++ = *ptr++; + } else { + ptr++; + switch (*ptr) { + case 'b': *ptr2++ = '\b'; break; + case 'f': *ptr2++ = '\f'; break; + case 'n': *ptr2++ = '\n'; break; + case 'r': *ptr2++ = '\r'; break; + case 't': *ptr2++ = '\t'; break; + default: *ptr2++ = *ptr; break; + } + ptr++; + } + } + + *ptr2 = '\0'; + item->valuestring = out; + item->type = cJSON_String; + + return ptr + 1; +} + +static const char *skip(const char *in) +{ + while (in && *in && ((unsigned char)*in <= 32)) { + in++; + } + return in; +} + +/* Forward declarations */ +static const char *parse_value(cJSON *item, const char *value); +static const char *parse_array(cJSON *item, const char *value); +static const char *parse_object(cJSON *item, const char *value); + +/* Parse an object */ +static const char *parse_object(cJSON *item, const char *value) +{ + cJSON *child = NULL; + cJSON *new_item = NULL; + + if (*value != '{') { + return NULL; + } + + item->type = cJSON_Object; + value = skip(value + 1); + + if (*value == '}') { + return value + 1; + } + + while (1) { + new_item = (cJSON *)malloc(sizeof(cJSON)); + if (!new_item) { + return NULL; + } + memset(new_item, 0, sizeof(cJSON)); + + if (!child) { + item->child = new_item; + } else { + child->next = new_item; + new_item->prev = child; + } + child = new_item; + + /* Parse name */ + value = skip(parse_string(child, skip(value))); + if (!value) { + return NULL; + } + child->string = child->valuestring; + child->valuestring = NULL; + + value = skip(value); + if (*value != ':') { + return NULL; + } + value = skip(parse_value(child, skip(value + 1))); + if (!value) { + return NULL; + } + + if (*value == '}') { + return value + 1; + } + + if (*value != ',') { + return NULL; + } + value = skip(value + 1); + } +} + +/* Parse an array */ +static const char *parse_array(cJSON *item, const char *value) +{ + cJSON *child = NULL; + cJSON *new_item = NULL; + + if (*value != '[') { + return NULL; + } + + item->type = cJSON_Array; + value = skip(value + 1); + + if (*value == ']') { + return value + 1; + } + + while (1) { + new_item = (cJSON *)malloc(sizeof(cJSON)); + if (!new_item) { + return NULL; + } + memset(new_item, 0, sizeof(cJSON)); + + if (!child) { + item->child = new_item; + } else { + child->next = new_item; + new_item->prev = child; + } + child = new_item; + + value = skip(parse_value(child, skip(value))); + if (!value) { + return NULL; + } + + if (*value == ']') { + return value + 1; + } + + if (*value != ',') { + return NULL; + } + value = skip(value + 1); + } +} + +/* Parse a value */ +static const char *parse_value(cJSON *item, const char *value) +{ + if (!value) { + return NULL; + } + + if (!strncmp(value, "null", 4)) { + item->type = cJSON_NULL; + return value + 4; + } + + if (!strncmp(value, "false", 5)) { + item->type = cJSON_False; + return value + 5; + } + + if (!strncmp(value, "true", 4)) { + item->type = cJSON_True; + item->valueint = 1; + return value + 4; + } + + if (*value == '\"') { + return parse_string(item, value); + } + + if ((*value == '-') || ((*value >= '0') && (*value <= '9'))) { + return parse_number(item, value); + } + + if (*value == '[') { + return parse_array(item, value); + } + + if (*value == '{') { + return parse_object(item, value); + } + + return NULL; +} + +/* Public parse function */ +cJSON *cJSON_Parse(const char *value) +{ + cJSON *c = (cJSON *)malloc(sizeof(cJSON)); + if (!c) { + return NULL; + } + memset(c, 0, sizeof(cJSON)); + + if (!parse_value(c, skip(value))) { + cJSON_Delete(c); + return NULL; + } + + return c; +} + +/* Delete cJSON object */ +void cJSON_Delete(cJSON *c) +{ + cJSON *next; + while (c) { + next = c->next; + if (c->child) { + cJSON_Delete(c->child); + } + if (c->valuestring) { + free(c->valuestring); + } + if (c->string) { + free(c->string); + } + free(c); + c = next; + } +} + +/* Get object item */ +cJSON *cJSON_GetObjectItem(const cJSON * const object, const char * const string) +{ + cJSON *c = object ? object->child : NULL; + while (c) { + if (c->string && !strcmp(c->string, string)) { + return c; + } + c = c->next; + } + return NULL; +} + +/* Get array item */ +cJSON *cJSON_GetArrayItem(const cJSON *array, int index) +{ + cJSON *c = array ? array->child : NULL; + while (c && index > 0) { + c = c->next; + index--; + } + return c; +} + +/* Get array size */ +int cJSON_GetArraySize(const cJSON *array) +{ + int size = 0; + cJSON *c = array ? array->child : NULL; + while (c) { + size++; + c = c->next; + } + return size; +} + +/* Get string value */ +const char *cJSON_GetStringValue(const cJSON * const item) +{ + if (!item || !(item->type & cJSON_String)) { + return NULL; + } + return item->valuestring; +} + +/* Get number value */ +double cJSON_GetNumberValue(const cJSON * const item) +{ + if (!item || !(item->type & cJSON_Number)) { + return 0; + } + return item->valuedouble; +} + +/* Type check functions */ +int cJSON_IsObject(const cJSON * const item) +{ + return item && (item->type == cJSON_Object); +} + +int cJSON_IsArray(const cJSON * const item) +{ + return item && (item->type == cJSON_Array); +} + +int cJSON_IsString(const cJSON * const item) +{ + return item && (item->type == cJSON_String); +} + +int cJSON_IsNumber(const cJSON * const item) +{ + return item && (item->type == cJSON_Number); +} + +int cJSON_IsBool(const cJSON * const item) +{ + return item && ((item->type == cJSON_True) || (item->type == cJSON_False)); +} diff --git a/src/prometheus_px4/src/modules/offline_task_manager/cJSON.h b/src/prometheus_px4/src/modules/offline_task_manager/cJSON.h new file mode 100644 index 0000000..da9e701 --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/cJSON.h @@ -0,0 +1,73 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef cJSON__h +#define cJSON__h + +#ifdef __cplusplus +extern "C" +{ +#endif + +/* cJSON Types: */ +#define cJSON_Invalid (0) +#define cJSON_False (1 << 0) +#define cJSON_True (1 << 1) +#define cJSON_NULL (1 << 2) +#define cJSON_Number (1 << 3) +#define cJSON_String (1 << 4) +#define cJSON_Array (1 << 5) +#define cJSON_Object (1 << 6) +#define cJSON_Raw (1 << 7) + +/* The cJSON structure: */ +typedef struct cJSON +{ + struct cJSON *next; + struct cJSON *prev; + struct cJSON *child; + int type; + char *valuestring; + int valueint; + double valuedouble; + char *string; +} cJSON; + +/* Functions used for minimal JSON parsing */ +cJSON *cJSON_Parse(const char *value); +void cJSON_Delete(cJSON *c); +cJSON *cJSON_GetObjectItem(const cJSON * const object, const char * const string); +cJSON *cJSON_GetArrayItem(const cJSON *array, int index); +int cJSON_GetArraySize(const cJSON *array); +const char *cJSON_GetStringValue(const cJSON * const item); +double cJSON_GetNumberValue(const cJSON * const item); +int cJSON_IsObject(const cJSON * const item); +int cJSON_IsArray(const cJSON * const item); +int cJSON_IsString(const cJSON * const item); +int cJSON_IsNumber(const cJSON * const item); +int cJSON_IsBool(const cJSON * const item); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/prometheus_px4/src/modules/offline_task_manager/offline_task.hpp b/src/prometheus_px4/src/modules/offline_task_manager/offline_task.hpp new file mode 100644 index 0000000..9d2766e --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/offline_task.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 offline_task.hpp + * + * Offline task data structures for offline_task_manager module + */ + +#pragma once + +#include +#include + +namespace offline_task_manager +{ + +/** + * @brief Single offline task definition + */ +struct OfflineTask { + int32_t task_id; double lat; + double lon; + float alt; + int32_t priority; + bool must_first; + bool reachable; + bool selected; +}; + +/** + * @brief Planned task with distance information + */ +struct PlannedTask { + OfflineTask task; + float distance_from_prev_m; +}; + +/** + * @brief Mission JSON structure + */ +struct OfflineMission { + int32_t version; + char mission_id[64]; + char start_mode[32]; + OfflineTask tasks[32]; // Max 32 tasks + int32_t task_count; +}; + +} // namespace offline_task_manager diff --git a/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager.cpp b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager.cpp new file mode 100644 index 0000000..c61a992 --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager.cpp @@ -0,0 +1,692 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 offline_task_manager.cpp + * + * Offline Task Manager implementation + */ + +#include "offline_task_manager.hpp" + +#include +#include +#include +#include +#include + +/* Task file path */ +#define OTM_TASK_FILE_PATH "/tmp/offline_tasks.json" + +namespace offline_task_manager + +{ + +OfflineTaskManager::OfflineTaskManager() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ + _cycle_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"); + + /* Initialize simulated battery from parameter */ + _sim_battery_percent = _param_otm_batt_init.get(); + _last_battery_update = hrt_absolute_time(); + + PX4_INFO("offline_task_manager initialized, sim battery: %.1f%%", (double)_sim_battery_percent); +} + +OfflineTaskManager::~OfflineTaskManager() +{ + perf_free(_cycle_perf); +} + +void OfflineTaskManager::Run() +{ + if (_task_should_exit.load()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + /* Update parameters */ + ModuleParams::updateParams(); + + /* Update simulated battery */ + update_simulated_battery(); + + /* Update uORB subscriptions */ + update_subscriptions(); + + /* Check if module is enabled */ + if (_param_otm_enable.get() == 0) { + if (_current_state != OTMState::IDLE) { + PX4_INFO("module disabled, returning to idle"); + reset_state_machine(); + } + perf_end(_cycle_perf); + ScheduleDelayed(1000000); /* 1 second delay when idle */ + return; + } + + /* Check for start trigger */ + if (_param_otm_start.get() == 0) { + if (_current_state != OTMState::IDLE && _current_state != OTMState::COMPLETED + && _current_state != OTMState::FAILED) { + PX4_INFO("start disabled, returning to idle"); + reset_state_machine(); + } + perf_end(_cycle_perf); + ScheduleDelayed(500000); /* 0.5 second delay when waiting */ + return; + } + + /* State machine execution */ + switch (_current_state) { + case OTMState::IDLE: + /* Transition to wait for home when start is enabled */ + if (_param_otm_start.get() != 0) { + PX4_INFO("starting offline task execution"); + transition_to_state(OTMState::WAIT_FOR_HOME); + } + break; + + case OTMState::WAIT_FOR_HOME: + if (home_position_valid()) { + PX4_INFO("home position valid (%.6f, %.6f)", + (double)_home_pos.lat, (double)_home_pos.lon); + transition_to_state(OTMState::WAIT_FOR_POSITION); + } else { + static hrt_abstime last_warn = 0; + hrt_abstime now = hrt_absolute_time(); + if (now - last_warn > 3000000) { /* 3 seconds */ + PX4_INFO("waiting for home position..."); + last_warn = now; + } + } + break; + + case OTMState::WAIT_FOR_POSITION: + if (global_position_valid()) { + PX4_INFO("global position valid (%.6f, %.6f)", + (double)_global_pos.lat, (double)_global_pos.lon); + transition_to_state(OTMState::LOAD_TASK_FILE); + } else { + static hrt_abstime last_warn = 0; + hrt_abstime now = hrt_absolute_time(); + if (now - last_warn > 3000000) { /* 3 seconds */ + PX4_INFO("waiting for global position..."); + last_warn = now; + } + } + break; + + case OTMState::LOAD_TASK_FILE: + if (load_tasks()) { + transition_to_state(OTMState::VALIDATE_TASKS); + } else { + transition_to_state(OTMState::FAILED); + } + break; + + case OTMState::VALIDATE_TASKS: + if (_raw_task_count > 0) { + PX4_INFO("validated %d tasks", _raw_task_count); + transition_to_state(OTMState::PLAN_TASKS); + } else { + PX4_ERR("no valid tasks to execute"); + transition_to_state(OTMState::FAILED); + } + break; + + case OTMState::PLAN_TASKS: + if (plan_tasks()) { + transition_to_state(OTMState::WRITE_MISSION); + } else { + PX4_ERR("task planning failed"); + transition_to_state(OTMState::FAILED); + } + break; + + case OTMState::WRITE_MISSION: + if (write_mission_to_dataman()) { + transition_to_state(OTMState::START_MISSION); + } else { + PX4_ERR("mission writing failed"); + transition_to_state(OTMState::FAILED); + } + break; + + case OTMState::START_MISSION: + start_mission_execution(); + transition_to_state(OTMState::MONITOR_MISSION); + break; + + case OTMState::MONITOR_MISSION: + /* TODO: Implement mission monitoring + * For now, just stay in this state and print status periodically + */ + { + static hrt_abstime last_status = 0; + hrt_abstime now = hrt_absolute_time(); + if (now - last_status > 5000000) { /* 5 seconds */ + PX4_INFO("mission monitoring: %d tasks planned, battery: %.1f%%", + _planned_task_count, (double)get_battery_percentage()); + last_status = now; + } + } + /* TODO: Detect mission completion and transition to COMPLETED */ + break; + + case OTMState::COMPLETED: + /* Mission completed successfully - stay here */ + break; + + case OTMState::FAILED: + /* Mission failed - stay here for debugging */ + break; + } + + perf_end(_cycle_perf); + + /* Schedule next run - 100ms when active, 1s when idle */ + if (_current_state == OTMState::IDLE || + _current_state == OTMState::COMPLETED || + _current_state == OTMState::FAILED) { + ScheduleDelayed(1000000); + } else { + ScheduleDelayed(100000); + } +} + +void OfflineTaskManager::update_simulated_battery() +{ + if (_param_otm_batt_sim.get() == 0) { + /* Not using simulation - battery updated from subscription */ + return; + } + + hrt_abstime now = hrt_absolute_time(); + float elapsed_seconds = (now - _last_battery_update) / 1000000.0f; + + if (elapsed_seconds > 0.0f) { + float drain = _param_otm_batt_drain.get() * elapsed_seconds; + _sim_battery_percent -= drain; + + /* Clamp to 0-100 range */ + if (_sim_battery_percent < 0.0f) { + _sim_battery_percent = 0.0f; + } + if (_sim_battery_percent > 100.0f) { + _sim_battery_percent = 100.0f; + } + } + + _last_battery_update = now; +} + +float OfflineTaskManager::get_battery_percentage() +{ + if (_param_otm_batt_sim.get() != 0) { + return _sim_battery_percent; + } + + /* Use real battery status */ + if (_battery_status_sub.update(&_battery_status)) { + /* battery_status.remaining is 0.0 to 1.0 */ + return _battery_status.remaining * 100.0f; + } + + /* No update available - return last known value or warning */ + static bool warned = false; + if (!warned) { + PX4_WARN("no battery status available, using last known value"); + warned = true; + } + + return _sim_battery_percent; +} + +void OfflineTaskManager::update_subscriptions() +{ + _battery_status_sub.update(&_battery_status); + _global_pos_sub.update(&_global_pos); + _home_pos_sub.update(&_home_pos); +} + +bool OfflineTaskManager::home_position_valid() const +{ + return _home_pos.valid_hpos; +} + +bool OfflineTaskManager::global_position_valid() const +{ + return _global_pos.timestamp > 0 && + PX4_ISFINITE(_global_pos.lat) && + PX4_ISFINITE(_global_pos.lon); +} + +bool OfflineTaskManager::get_current_position(double &lat, double &lon) const +{ + if (global_position_valid()) { + lat = _global_pos.lat; + lon = _global_pos.lon; + return true; + } + return false; +} + +bool OfflineTaskManager::load_tasks() +{ + PX4_INFO("loading tasks from %s", OTM_TASK_FILE_PATH); + + int max_tasks = _param_otm_max_tasks.get(); + if (max_tasks > MAX_TASKS) { + max_tasks = MAX_TASKS; + } + + bool success = load_offline_tasks_from_json( + OTM_TASK_FILE_PATH, + _raw_tasks, + max_tasks, + _raw_task_count); + + return success; +} + +bool OfflineTaskManager::plan_tasks() +{ + double start_lat, start_lon; + if (!get_current_position(start_lat, start_lon)) { + PX4_ERR("cannot get current position for planning"); + return false; + } + + float battery = get_battery_percentage(); + float low_batt = _param_otm_low_batt.get(); + + PX4_INFO("starting task planning: battery=%.1f%% low_thresh=%.1f%%", + (double)battery, (double)low_batt); + + int max_planned = _param_otm_max_tasks.get(); + if (max_planned > MAX_TASKS) { + max_planned = MAX_TASKS; + } + + int result = plan_offline_tasks( + _raw_tasks, + _raw_task_count, + start_lat, + start_lon, + battery, + low_batt, + _planned_tasks, + max_planned, + _total_planned_distance); + + if (result < 0) { + PX4_ERR("task planning failed"); + return false; + } + + _planned_task_count = result; + return true; +} + +bool OfflineTaskManager::write_mission_to_dataman() +{ + if (_planned_task_count <= 0) { + PX4_ERR("no tasks to write"); + return false; + } + + PX4_INFO("writing %d mission items to dataman", _planned_task_count); + + dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0; + + /* Lock mission state for atomic update */ + int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + + /* Write each mission item */ + for (int i = 0; i < _planned_task_count; i++) { + mission_item_s item{}; + + item.nav_cmd = NAV_CMD_WAYPOINT; + item.lat = _planned_tasks[i].lat; + item.lon = _planned_tasks[i].lon; + item.altitude = _planned_tasks[i].alt; + item.altitude_is_relative = false; + item.acceptance_radius = _param_otm_acc_rad.get(); + item.time_inside = _param_otm_hold_t.get(); + item.loiter_radius = 0.0f; + item.yaw = NAN; + item.autocontinue = true; + item.origin = ORIGIN_ONBOARD; + item.frame = NAV_FRAME_GLOBAL; + + /* Write to dataman */ + ssize_t written = dm_write( + dm_item, + i, + DM_PERSIST_POWER_ON_RESET, + &item, + sizeof(mission_item_s)); + + if (written != sizeof(mission_item_s)) { + PX4_ERR("failed to write mission item %d", i); + if (dm_lock_ret == 0) { + dm_unlock(DM_KEY_MISSION_STATE); + } + return false; + } + + PX4_DEBUG("wrote mission item %d: (%.6f, %.6f) alt=%.1f", + i, (double)item.lat, (double)item.lon, (double)item.altitude); + } + + /* Create and write mission state */ + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = (uint8_t)dm_item; + mission.count = (uint16_t)_planned_task_count; + mission.current_seq = 0; + + ssize_t res = dm_write( + DM_KEY_MISSION_STATE, + 0, + DM_PERSIST_POWER_ON_RESET, + &mission, + sizeof(mission_s)); + + if (dm_lock_ret == 0) { + dm_unlock(DM_KEY_MISSION_STATE); + } + + if (res != sizeof(mission_s)) { + PX4_ERR("failed to write mission state"); + return false; + } + + /* Publish mission topic to notify navigator */ + _mission_pub.publish(mission); + + _mission_written = true; + + PX4_INFO("offline mission published: count=%d dataman_id=%d", + _planned_task_count, (int)dm_item); + + return true; +} + +void OfflineTaskManager::start_mission_execution() +{ + PX4_INFO("sending ARM and AUTO.MISSION commands"); + + /* Step 1: Send ARM command */ + vehicle_command_s arm_cmd{}; + arm_cmd.timestamp = hrt_absolute_time(); + arm_cmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; + arm_cmd.param1 = 1.0f; /* 1 = arm, 0 = disarm */ + arm_cmd.param2 = 0.0f; /* 0 = normal arm, 21196 = force arm (not recommended) */ + arm_cmd.target_system = 1; + arm_cmd.target_component = 1; + arm_cmd.source_system = 1; + arm_cmd.source_component = 1; + arm_cmd.from_external = false; + + _vehicle_cmd_pub.publish(arm_cmd); + PX4_INFO("arm command sent"); + + /* Step 2: Send MISSION mode command */ + vehicle_command_s mode_cmd{}; + mode_cmd.timestamp = hrt_absolute_time(); + mode_cmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; + mode_cmd.param1 = 1.0f; + mode_cmd.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; + mode_cmd.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + mode_cmd.target_system = 1; + mode_cmd.target_component = 1; + mode_cmd.source_system = 1; + mode_cmd.source_component = 1; + mode_cmd.from_external = false; + + _vehicle_cmd_pub.publish(mode_cmd); + PX4_INFO("AUTO.MISSION command sent"); + + _mission_started = true; +} + +void OfflineTaskManager::transition_to_state(OTMState new_state) +{ + PX4_INFO("state transition: %s -> %s", + state_to_string(_current_state), + state_to_string(new_state)); + + _current_state = new_state; +} + +void OfflineTaskManager::reset_state_machine() +{ + _current_state = OTMState::IDLE; + _mission_written = false; + _mission_started = false; + _raw_task_count = 0; + _planned_task_count = 0; +} + +const char *OfflineTaskManager::state_to_string(OTMState state) +{ + switch (state) { + case OTMState::IDLE: + return "IDLE"; + case OTMState::WAIT_FOR_HOME: + return "WAIT_FOR_HOME"; + case OTMState::WAIT_FOR_POSITION: + return "WAIT_FOR_POSITION"; + case OTMState::LOAD_TASK_FILE: + return "LOAD_TASK_FILE"; + case OTMState::VALIDATE_TASKS: + return "VALIDATE_TASKS"; + case OTMState::PLAN_TASKS: + return "PLAN_TASKS"; + case OTMState::WRITE_MISSION: + return "WRITE_MISSION"; + case OTMState::START_MISSION: + return "START_MISSION"; + case OTMState::MONITOR_MISSION: + return "MONITOR_MISSION"; + case OTMState::COMPLETED: + return "COMPLETED"; + case OTMState::FAILED: + return "FAILED"; + default: + return "UNKNOWN"; + } +} + +int OfflineTaskManager::print_status() +{ + PX4_INFO("Offline Task Manager Status:"); + PX4_INFO(" State: %s", state_to_string(_current_state)); + PX4_INFO(" Enabled: %s", _param_otm_enable.get() ? "yes" : "no"); + PX4_INFO(" Start: %s", _param_otm_start.get() ? "yes" : "no"); + PX4_INFO(" Battery: %.1f%%", (double)get_battery_percentage()); + PX4_INFO(" Home valid: %s", home_position_valid() ? "yes" : "no"); + PX4_INFO(" Position valid: %s", global_position_valid() ? "yes" : "no"); + PX4_INFO(" Raw tasks: %d", _raw_task_count); + PX4_INFO(" Planned tasks: %d", _planned_task_count); + PX4_INFO(" Mission written: %s", _mission_written ? "yes" : "no"); + PX4_INFO(" Mission started: %s", _mission_started ? "yes" : "no"); + + if (_planned_task_count > 0) { + PX4_INFO(" Planned distance: %.1fm", (double)_total_planned_distance); + } + + perf_print_counter(_cycle_perf); + + return 0; +} + +/* Static module interface functions */ + +int OfflineTaskManager::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Offline Task Manager module for autonomous drone missions. + +This module reads task points from a local JSON file, plans the optimal +execution path based on task priority and battery level, generates PX4 +mission items, and switches to AUTO.MISSION mode for execution. + +### Features +- JSON task file parsing +- Priority-based task filtering +- Nearest neighbor path planning +- Battery-aware task selection +- Mission item generation and dataman storage +- Auto mode switching + +### Usage +Start the module: +$ offline_task_manager start + +Enable and start execution: +$ param set OTM_ENABLE 1 +$ param set OTM_START 1 + +Check status: +$ offline_task_manager status + +Stop the module: +$ offline_task_manager stop + +### JSON File Format +Create /tmp/offline_tasks.json: +{ + "version": 1, + "mission_id": "test_mission", + "start_mode": "offline_auto", + "tasks": [ + { + "task_id": 1, + "lat": 47.3978, + "lon": 8.5456, + "alt": 520.0, + "priority": 5, + "must_first": true + } + ] +} +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("offline_task_manager", "module"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("Start the background task", nullptr, false); + + PRINT_MODULE_USAGE_COMMAND("stop"); + PRINT_MODULE_USAGE_ARG("Stop the background task", nullptr, false); + + PRINT_MODULE_USAGE_COMMAND("status"); + PRINT_MODULE_USAGE_ARG("Print module status", nullptr, false); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +int OfflineTaskManager::custom_command(int argc, char *argv[]) +{ + if (!strcmp(argv[0], "start")) { + return task_spawn(argc, argv); + } + + if (!is_running()) { + print_usage("module not running"); + return 1; + } + + if (!strcmp(argv[0], "stop")) { + get_instance()->request_stop(); + return 0; + } + + if (!strcmp(argv[0], "status")) { + return get_instance()->print_status(); + } + + print_usage("unknown command"); + return 1; +} + +int OfflineTaskManager::task_spawn(int argc, char *argv[]) +{ + // For ScheduledWorkItem modules, create instance directly and schedule it + OfflineTaskManager *instance = new OfflineTaskManager(); + + if (!instance) { + PX4_ERR("alloc failed"); + return 1; + } + + if (instance->init() != 0) { + PX4_ERR("init failed"); + delete instance; + return 1; + } + + // Store in ModuleBase (atomic) + _object.store(instance); + + // Mark as running on work queue (required for is_running() to return true) + _task_id = task_id_is_work_queue; + + // Schedule the work item (100ms interval) + instance->ScheduleOnInterval(100000); + + return 0; +} + +} /* namespace offline_task_manager */ + +/* Module entry point */ +extern "C" __EXPORT int offline_task_manager_main(int argc, char *argv[]) +{ + return offline_task_manager::OfflineTaskManager::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager.hpp b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager.hpp new file mode 100644 index 0000000..1bd8b61 --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager.hpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 offline_task_manager.hpp + * + * Offline Task Manager for autonomous drone missions + * + * This module provides offline task planning and execution for drones. + * It reads tasks from a local JSON file, plans the optimal path based on + * task priority and battery level, generates PX4 mission items, and + * switches to AUTO.MISSION mode for execution. + * + * @author PX4 Development Team + */ + +#pragma once + +#include "offline_task.hpp" +#include "offline_task_parser.hpp" +#include "offline_task_planner.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Include custom mode definitions for AUTO.MISSION */ +#include + +namespace offline_task_manager +{ + +/** + * @brief State machine states for offline task management + */ +enum class OTMState { + IDLE = 0, /**< Module is idle, waiting for enable */ + WAIT_FOR_HOME, /**< Waiting for home position to be valid */ + WAIT_FOR_POSITION, /**< Waiting for global position to be valid */ + LOAD_TASK_FILE, /**< Loading tasks from JSON file */ + VALIDATE_TASKS, /**< Validating loaded tasks */ + PLAN_TASKS, /**< Planning task execution order */ + WRITE_MISSION, /**< Writing mission items to dataman */ + START_MISSION, /**< Starting mission execution */ + MONITOR_MISSION, /**< Monitoring mission execution */ + COMPLETED, /**< Mission completed successfully */ + FAILED /**< Mission failed */ +}; + +/** + * @brief Offline Task Manager class + * + * Manages offline task planning and execution for drones. + */ +class OfflineTaskManager : public ModuleBase, + public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + /** + * @brief Constructor + */ + OfflineTaskManager(); + + /** + * @brief Destructor + */ + ~OfflineTaskManager() override; + + /** + * @brief Module instantiation (for ModuleBase compatibility) + */ + static OfflineTaskManager *instantiate(int argc, char *argv[]) + { + return new OfflineTaskManager(); + } + + /** + * @brief Module initialization + * @return 0 on success, -1 on failure + */ + int init() { return 0; } + + /** + * @brief Main module entry point + * @param argc Argument count + * @param argv Argument values + * @return 0 on success, 1 on failure + */ + static int task_spawn(int argc, char *argv[]); + + /** + * @brief Print module usage + */ + static int print_usage(const char *reason); + static int print_usage() { return print_usage(nullptr); } + + /** + * @brief Start the module + * @param argc Argument count + * @param argv Argument values + * @return 0 on success, 1 on failure + */ + static int custom_command(int argc, char *argv[]); + + /** + * @brief Print current module status + * @return 0 + */ + int print_status() override; + + /** + * @brief Request module to stop + */ + void request_stop() { _task_should_exit.store(true); } + +private: + /** + * @brief Scheduled work item run function + */ + void Run() override; + + /** + * @brief Update simulated battery level + */ + void update_simulated_battery(); + + /** + * @brief Get current battery percentage + * @return Battery percentage (0-100) + */ + float get_battery_percentage(); + + /** + * @brief Load tasks from JSON file + * @return true if successful + */ + bool load_tasks(); + + /** + * @brief Plan task execution order + * @return true if successful + */ + bool plan_tasks(); + + /** + * @brief Write planned mission to dataman + * @return true if successful + */ + bool write_mission_to_dataman(); + + /** + * @brief Start mission execution by sending AUTO.MISSION command + */ + void start_mission_execution(); + + /** + * @brief Get string representation of state + * @param state State enum value + * @return State name string + */ + const char *state_to_string(OTMState state); + + /** + * @brief Reset state machine to idle + */ + void reset_state_machine(); + + /** + * @brief Update uORB subscriptions + */ + void update_subscriptions(); + + /** + * @brief Check if home position is valid + * @return true if valid + */ + bool home_position_valid() const; + + /** + * @brief Check if global position is valid + * @return true if valid + */ + bool global_position_valid() const; + + /** + * @brief Get current position + * @param lat Output latitude + * @param lon Output longitude + * @return true if position available + */ + bool get_current_position(double &lat, double &lon) const; + + /** + * @brief State transition helper + * @param new_state State to transition to + */ + void transition_to_state(OTMState new_state); + + /* uORB Subscriptions */ + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; + + /* uORB Publications */ + uORB::Publication _mission_pub{ORB_ID(mission)}; + uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; + + /* Cached uORB data */ + battery_status_s _battery_status{}; + vehicle_global_position_s _global_pos{}; + home_position_s _home_pos{}; + + /* Simulated battery */ + float _sim_battery_percent{100.0f}; + hrt_abstime _last_battery_update{0}; + + /* Task data */ + static constexpr int MAX_TASKS = 32; + OfflineTask _raw_tasks[MAX_TASKS]; + int _raw_task_count{0}; + OfflineTask _planned_tasks[MAX_TASKS]; + int _planned_task_count{0}; + float _total_planned_distance{0.0f}; + + /* State machine */ + OTMState _current_state{OTMState::IDLE}; + bool _mission_written{false}; + bool _mission_started{false}; + + /* Module exit flag */ + px4::atomic_bool _task_should_exit{false}; + + /* Performance counter */ + perf_counter_t _cycle_perf{nullptr}; + + /* Parameters */ + DEFINE_PARAMETERS( + (ParamInt) _param_otm_enable, + (ParamInt) _param_otm_start, + (ParamFloat) _param_otm_hold_t, + (ParamFloat) _param_otm_acc_rad, + (ParamFloat) _param_otm_batt_init, + (ParamInt) _param_otm_batt_sim, + (ParamFloat) _param_otm_batt_drain, + (ParamFloat) _param_otm_low_batt, + (ParamInt) _param_otm_max_tasks + ); +}; + +} // namespace offline_task_manager diff --git a/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager_params.c b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager_params.c new file mode 100644 index 0000000..9bd6e80 --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_manager_params.c @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 offline_task_manager_params.c + * + * Parameters for offline_task_manager module + */ + +#include +#include + +/** + * Offline Task Manager Enable + * + * Enable the offline task manager module. + * + * @boolean + * @group Offline Task Manager + */ +PARAM_DEFINE_INT32(OTM_ENABLE, 0); + +/** + * Offline Task Manager Start + * + * Set to 1 to trigger offline task execution. + * Module will read tasks, plan, write mission and start execution. + * + * @boolean + * @group Offline Task Manager + */ +PARAM_DEFINE_INT32(OTM_START, 0); + +/** + * Task Point Hold Time + * + * Time to loiter at each task point after reaching it. + * + * @unit s + * @min 0 + * @max 300 + * @decimal 1 + * @group Offline Task Manager + */ +PARAM_DEFINE_FLOAT(OTM_HOLD_T, 10.0f); + +/** + * Task Point Acceptance Radius + * + * Radius within which a task point is considered reached. + * + * @unit m + * @min 0.5 + * @max 50 + * @decimal 1 + * @group Offline Task Manager + */ +PARAM_DEFINE_FLOAT(OTM_ACC_RAD, 2.0f); + +/** + * Simulated Battery Initial Percentage + * + * Initial battery percentage for simulation mode. + * + * @unit % + * @min 0 + * @max 100 + * @decimal 1 + * @group Offline Task Manager + */ +PARAM_DEFINE_FLOAT(OTM_BATT_INIT, 100.0f); + +/** + * Battery Simulation Mode + * + * Use simulated battery instead of real battery status. + * 0: Use real battery + * 1: Use simulated battery + * + * @boolean + * @group Offline Task Manager + */ +PARAM_DEFINE_INT32(OTM_BATT_SIM, 1); + +/** + * Simulated Battery Drain Rate + * + * Battery percentage drained per second in simulation. + * + * @min 0 + * @max 10 + * @decimal 3 + * @group Offline Task Manager + */ +PARAM_DEFINE_FLOAT(OTM_BATT_DRAIN, 0.1f); + +/** + * Low Battery Threshold + * + * Battery percentage below which only urgent tasks are executed. + * + * @unit % + * @min 0 + * @max 100 + * @decimal 1 + * @group Offline Task Manager + */ +PARAM_DEFINE_FLOAT(OTM_LOW_BATT, 20.0f); + +/** + * Maximum Number of Tasks + * + * Maximum number of tasks to load from JSON file. + * + * @min 1 + * @max 100 + * @group Offline Task Manager + */ +PARAM_DEFINE_INT32(OTM_MAX_TASKS, 32); diff --git a/src/prometheus_px4/src/modules/offline_task_manager/offline_task_parser.cpp b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_parser.cpp new file mode 100644 index 0000000..ff0f42b --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_parser.cpp @@ -0,0 +1,243 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 offline_task_parser.cpp + * + * JSON parser implementation for offline tasks + */ + +#include "offline_task_parser.hpp" +#include "cJSON.h" + +#include +#include +#include + +namespace offline_task_manager +{ + +static constexpr size_t MAX_FILE_SIZE = 65536; // 64KB max + +bool load_offline_tasks_from_json( + const char *file_path, + OfflineTask *tasks, + int max_tasks, + int &task_count) +{ + task_count = 0; + + if (!file_path || !tasks || max_tasks <= 0) { + PX4_ERR("invalid parameters"); + return false; + } + + /* Open file */ + FILE *fp = fopen(file_path, "r"); + if (!fp) { + PX4_ERR("failed to open file: %s", file_path); + return false; + } + + /* Get file size */ + fseek(fp, 0, SEEK_END); + long file_size = ftell(fp); + fseek(fp, 0, SEEK_SET); + + if (file_size <= 0 || file_size > (long)MAX_FILE_SIZE) { + PX4_ERR("invalid file size: %ld", file_size); + fclose(fp); + return false; + } + + /* Read file */ + char *buffer = new char[file_size + 1]; + if (!buffer) { + PX4_ERR("failed to allocate buffer"); + fclose(fp); + return false; + } + + size_t read_size = fread(buffer, 1, file_size, fp); + fclose(fp); + + if (read_size != (size_t)file_size) { + PX4_ERR("failed to read file completely"); + delete[] buffer; + return false; + } + + buffer[file_size] = '\0'; + + /* Parse JSON */ + cJSON *root = cJSON_Parse(buffer); + delete[] buffer; + + if (!root) { + PX4_ERR("failed to parse JSON"); + return false; + } + + if (!cJSON_IsObject(root)) { + PX4_ERR("JSON root is not an object"); + cJSON_Delete(root); + return false; + } + + /* Get tasks array */ + cJSON *tasks_array = cJSON_GetObjectItem(root, "tasks"); + if (!tasks_array) { + PX4_ERR("missing 'tasks' field"); + cJSON_Delete(root); + return false; + } + + if (!cJSON_IsArray(tasks_array)) { + PX4_ERR("'tasks' is not an array"); + cJSON_Delete(root); + return false; + } + + int array_size = cJSON_GetArraySize(tasks_array); + if (array_size <= 0) { + PX4_ERR("tasks array is empty"); + cJSON_Delete(root); + return false; + } + + if (array_size > max_tasks) { + PX4_WARN("tasks array truncated from %d to %d", array_size, max_tasks); + array_size = max_tasks; + } + + /* Parse each task */ + for (int i = 0; i < array_size; i++) { + cJSON *task_obj = cJSON_GetArrayItem(tasks_array, i); + if (!task_obj || !cJSON_IsObject(task_obj)) { + PX4_WARN("task %d is not an object, skipping", i); + continue; + } + + OfflineTask task{}; + + /* task_id (required) */ + cJSON *task_id = cJSON_GetObjectItem(task_obj, "task_id"); + if (!task_id || !cJSON_IsNumber(task_id)) { + PX4_WARN("task %d missing 'task_id', skipping", i); + continue; + } + task.task_id = (int32_t)cJSON_GetNumberValue(task_id); + + /* lat (required) */ + cJSON *lat = cJSON_GetObjectItem(task_obj, "lat"); + if (!lat || !cJSON_IsNumber(lat)) { + PX4_WARN("task %d missing 'lat', skipping", i); + continue; + } + task.lat = cJSON_GetNumberValue(lat); + if (task.lat < -90.0 || task.lat > 90.0) { + PX4_WARN("task %d invalid lat: %.6f, skipping", i, task.lat); + continue; + } + + /* lon (required) */ + cJSON *lon = cJSON_GetObjectItem(task_obj, "lon"); + if (!lon || !cJSON_IsNumber(lon)) { + PX4_WARN("task %d missing 'lon', skipping", i); + continue; + } + task.lon = cJSON_GetNumberValue(lon); + if (task.lon < -180.0 || task.lon > 180.0) { + PX4_WARN("task %d invalid lon: %.6f, skipping", i, task.lon); + continue; + } + + /* alt (required) */ + cJSON *alt = cJSON_GetObjectItem(task_obj, "alt"); + if (!alt || !cJSON_IsNumber(alt)) { + PX4_WARN("task %d missing 'alt', skipping", i); + continue; + } + task.alt = (float)cJSON_GetNumberValue(alt); + if (task.alt < 0.0f || task.alt > 5000.0f) { + PX4_WARN("task %d invalid alt: %.2f, skipping", i, task.alt); + continue; + } + + /* priority (required) */ + cJSON *priority = cJSON_GetObjectItem(task_obj, "priority"); + if (!priority || !cJSON_IsNumber(priority)) { + PX4_WARN("task %d missing 'priority', skipping", i); + continue; + } + task.priority = (int32_t)cJSON_GetNumberValue(priority); + if (task.priority < 1 || task.priority > 5) { + PX4_WARN("task %d invalid priority: %d, skipping", i, task.priority); + continue; + } + + /* must_first (optional, default false) */ + cJSON *must_first = cJSON_GetObjectItem(task_obj, "must_first"); + if (must_first && cJSON_IsBool(must_first)) { + task.must_first = cJSON_IsNumber(must_first) ? + (cJSON_GetNumberValue(must_first) != 0) : + cJSON_IsNumber(must_first); + } else { + task.must_first = false; + } + + /* Initialize other fields */ + task.reachable = true; + task.selected = false; + + /* Store task */ + tasks[task_count] = task; + task_count++; + + PX4_INFO("loaded task %d: id=%d lat=%.6f lon=%.6f alt=%.1f pri=%d must_first=%s", + i, task.task_id, task.lat, task.lon, task.alt, task.priority, + task.must_first ? "true" : "false"); + } + + cJSON_Delete(root); + + if (task_count == 0) { + PX4_ERR("no valid tasks loaded"); + return false; + } + + PX4_INFO("successfully loaded %d tasks", task_count); + return true; +} + +} // namespace offline_task_manager diff --git a/src/prometheus_px4/src/modules/offline_task_manager/offline_task_parser.hpp b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_parser.hpp new file mode 100644 index 0000000..6ec229d --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_parser.hpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 offline_task_parser.hpp + * + * JSON parser for offline tasks + */ + +#pragma once + +#include "offline_task.hpp" + +namespace offline_task_manager +{ + +/** + * @brief Load offline tasks from JSON file + * + * @param file_path Path to JSON file + * @param tasks Array to store loaded tasks + * @param max_tasks Maximum number of tasks to load + * @param task_count Output: actual number of tasks loaded + * @return true if successful, false otherwise + */ +bool load_offline_tasks_from_json( + const char *file_path, + OfflineTask *tasks, + int max_tasks, + int &task_count +); + +} // namespace offline_task_manager diff --git a/src/prometheus_px4/src/modules/offline_task_manager/offline_task_planner.cpp b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_planner.cpp new file mode 100644 index 0000000..6777b56 --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_planner.cpp @@ -0,0 +1,319 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 offline_task_planner.cpp + * + * Task planner implementation using nearest neighbor algorithm + */ + +#include "offline_task_planner.hpp" +#include +#include + +namespace offline_task_manager +{ + +/* Earth radius in meters */ +static constexpr double EARTH_RADIUS_M = 6371000.0; + +/* Degrees to radians */ +static double deg2rad(double deg) +{ + return deg * M_PI / 180.0; +} + +float haversine_distance(double lat1, double lon1, double lat2, double lon2) +{ + double d_lat = deg2rad(lat2 - lat1); + double d_lon = deg2rad(lon2 - lon1); + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + + cos(deg2rad(lat1)) * cos(deg2rad(lat2)) * + sin(d_lon / 2.0) * sin(d_lon / 2.0); + + double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a)); + + return (float)(EARTH_RADIUS_M * c); +} + +/* Find nearest unselected task using nearest neighbor algorithm */ +static int find_nearest_task( + const OfflineTask *tasks, + int task_count, + double current_lat, + double current_lon, + bool *selected_mask) +{ + float min_distance = -1.0f; + int nearest_idx = -1; + + for (int i = 0; i < task_count; i++) { + if (selected_mask[i]) { + continue; + } + + float dist = haversine_distance( + current_lat, current_lon, + tasks[i].lat, tasks[i].lon); + + if (nearest_idx < 0 || dist < min_distance) { + min_distance = dist; + nearest_idx = i; + } + } + + return nearest_idx; +} + +/* Nearest neighbor planning for a subset of tasks */ +static int nearest_neighbor_plan( + const OfflineTask *source_tasks, + int source_count, + double start_lat, + double start_lon, + OfflineTask *planned_tasks, + int max_planned, + float &total_distance) +{ + if (source_count <= 0 || max_planned <= 0) { + return 0; + } + + /* Track which tasks are selected */ + bool *selected = new bool[source_count]; + if (!selected) { + return -1; + } + + for (int i = 0; i < source_count; i++) { + selected[i] = false; + } + + double current_lat = start_lat; + double current_lon = start_lon; + total_distance = 0.0f; + int planned_count = 0; + + /* Greedy nearest neighbor selection */ + while (planned_count < source_count && planned_count < max_planned) { + int nearest_idx = find_nearest_task( + source_tasks, source_count, + current_lat, current_lon, + selected); + + if (nearest_idx < 0) { + break; + } + + /* Calculate distance to this task */ + float dist = haversine_distance( + current_lat, current_lon, + source_tasks[nearest_idx].lat, + source_tasks[nearest_idx].lon); + + /* Add to plan */ + planned_tasks[planned_count] = source_tasks[nearest_idx]; + total_distance += dist; + + PX4_DEBUG("planned task %d: id=%d dist=%.1fm (lat=%.6f lon=%.6f)", + planned_count, + source_tasks[nearest_idx].task_id, + dist, + source_tasks[nearest_idx].lat, + source_tasks[nearest_idx].lon); + + /* Update current position to this task */ + current_lat = source_tasks[nearest_idx].lat; + current_lon = source_tasks[nearest_idx].lon; + selected[nearest_idx] = true; + planned_count++; + } + + delete[] selected; + + PX4_INFO("nearest neighbor planned %d tasks, total distance: %.1fm", + planned_count, (double)total_distance); + + return planned_count; +} + +int plan_offline_tasks( + const OfflineTask *input_tasks, + int input_count, + double start_lat, + double start_lon, + float battery_percent, + float low_batt_threshold, + OfflineTask *planned_tasks, + int max_planned_tasks, + float &total_distance_m) +{ + if (!input_tasks || !planned_tasks || input_count <= 0 || max_planned_tasks <= 0) { + PX4_ERR("invalid planner parameters"); + return -1; + } + + PX4_INFO("planning %d tasks from (%.6f, %.6f), battery: %.1f%%", + input_count, start_lat, start_lon, (double)battery_percent); + + /* Determine which tasks to include based on battery */ + bool low_battery = (battery_percent < low_batt_threshold); + + if (low_battery) { + PX4_WARN("low battery mode: only urgent tasks will be planned"); + } + + /* Separate urgent and normal tasks */ + OfflineTask *urgent_tasks = new OfflineTask[input_count]; + OfflineTask *normal_tasks = new OfflineTask[input_count]; + + if (!urgent_tasks || !normal_tasks) { + PX4_ERR("failed to allocate task buffers"); + delete[] urgent_tasks; + delete[] normal_tasks; + return -1; + } + + int urgent_count = 0; + int normal_count = 0; + + for (int i = 0; i < input_count; i++) { + bool is_urgent = (input_tasks[i].must_first || input_tasks[i].priority == 5); + + if (is_urgent) { + urgent_tasks[urgent_count++] = input_tasks[i]; + PX4_DEBUG("task %d is urgent (must_first=%s priority=%d)", + input_tasks[i].task_id, + input_tasks[i].must_first ? "true" : "false", + input_tasks[i].priority); + } else if (!low_battery) { + /* Only include normal tasks if not in low battery mode */ + normal_tasks[normal_count++] = input_tasks[i]; + } + } + + PX4_INFO("urgent tasks: %d, normal tasks: %d", urgent_count, normal_count); + + total_distance_m = 0.0f; + int total_planned = 0; + double current_lat = start_lat; + double current_lon = start_lon; + + /* Plan urgent tasks first using nearest neighbor */ + if (urgent_count > 0) { + PX4_INFO("planning urgent tasks..."); + + OfflineTask *planned_urgent = new OfflineTask[urgent_count]; + if (!planned_urgent) { + PX4_ERR("failed to allocate urgent plan buffer"); + delete[] urgent_tasks; + delete[] normal_tasks; + return -1; + } + + float urgent_distance = 0.0f; + int planned_urgent_count = nearest_neighbor_plan( + urgent_tasks, urgent_count, + current_lat, current_lon, + planned_urgent, urgent_count, + urgent_distance); + + /* Copy urgent tasks to output */ + for (int i = 0; i < planned_urgent_count && total_planned < max_planned_tasks; i++) { + planned_tasks[total_planned++] = planned_urgent[i]; + } + + total_distance_m += urgent_distance; + + /* Update current position to last urgent task */ + if (planned_urgent_count > 0) { + current_lat = planned_urgent[planned_urgent_count - 1].lat; + current_lon = planned_urgent[planned_urgent_count - 1].lon; + } + + delete[] planned_urgent; + } + + /* Plan normal tasks if not in low battery mode */ + if (normal_count > 0 && !low_battery && total_planned < max_planned_tasks) { + PX4_INFO("planning normal tasks..."); + + int remaining_slots = max_planned_tasks - total_planned; + OfflineTask *planned_normal = new OfflineTask[normal_count]; + if (!planned_normal) { + PX4_ERR("failed to allocate normal plan buffer"); + delete[] urgent_tasks; + delete[] normal_tasks; + return total_planned; + } + + float normal_distance = 0.0f; + int planned_normal_count = nearest_neighbor_plan( + normal_tasks, normal_count, + current_lat, current_lon, + planned_normal, remaining_slots, + normal_distance); + + /* Copy normal tasks to output */ + for (int i = 0; i < planned_normal_count && total_planned < max_planned_tasks; i++) { + planned_tasks[total_planned++] = planned_normal[i]; + } + + total_distance_m += normal_distance; + + delete[] planned_normal; + } + + /* Cleanup */ + delete[] urgent_tasks; + delete[] normal_tasks; + + PX4_INFO("planning complete: %d tasks, total distance: %.1fm", + total_planned, (double)total_distance_m); + + /* Print planned sequence */ + for (int i = 0; i < total_planned; i++) { + PX4_INFO(" [%d] task_id=%d (%.6f, %.6f) alt=%.1f priority=%d", + i, + planned_tasks[i].task_id, + planned_tasks[i].lat, + planned_tasks[i].lon, + (double)planned_tasks[i].alt, + planned_tasks[i].priority); + } + + return total_planned; +} + +} // namespace offline_task_manager diff --git a/src/prometheus_px4/src/modules/offline_task_manager/offline_task_planner.hpp b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_planner.hpp new file mode 100644 index 0000000..730b5f7 --- /dev/null +++ b/src/prometheus_px4/src/modules/offline_task_manager/offline_task_planner.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 offline_task_planner.hpp + * + * Task planner for offline missions + */ + +#pragma once + +#include "offline_task.hpp" + +namespace offline_task_manager +{ + +/** + * @brief Plan offline tasks using nearest neighbor algorithm + * + * @param input_tasks Input tasks array + * @param input_count Number of input tasks + * @param start_lat Starting latitude (current position) + * @param start_lon Starting longitude (current position) + * @param battery_percent Current battery percentage + * @param low_batt_threshold Low battery threshold + * @param planned_tasks Output array for planned tasks + * @param max_planned_tasks Maximum number of planned tasks + * @param total_distance_m Output total distance in meters + * @return Number of planned tasks, -1 on error + */ +int plan_offline_tasks( + const OfflineTask *input_tasks, + int input_count, + double start_lat, + double start_lon, + float battery_percent, + float low_batt_threshold, + OfflineTask *planned_tasks, + int max_planned_tasks, + float &total_distance_m +); + +/** + * @brief Calculate Haversine distance between two points + * + * @param lat1 Latitude of point 1 in degrees + * @param lon1 Longitude of point 1 in degrees + * @param lat2 Latitude of point 2 in degrees + * @param lon2 Longitude of point 2 in degrees + * @return Distance in meters + */ +float haversine_distance(double lat1, double lon1, double lat2, double lon2); + +} // namespace offline_task_manager diff --git a/src/prometheus_px4/src/modules/px4iofirmware/CMakeLists.txt b/src/prometheus_px4/src/modules/px4iofirmware/CMakeLists.txt new file mode 100644 index 0000000..3b76556 --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/CMakeLists.txt @@ -0,0 +1,61 @@ +############################################################################ +# +# 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. +# +############################################################################ + +option(PX4IO_PERF "Enable px4io perf counters" OFF) + +add_library(px4iofirmware + adc.c + controls.c + mixer.cpp + px4io.c + registers.c + safety.c + serial.c +) + +set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES px4iofirmware) +target_link_libraries(px4iofirmware + PUBLIC + arch_io_pins + nuttx_apps + nuttx_arch + nuttx_c + mixer + rc + output_limit +) + +if(PX4IO_PERF) + target_compile_definitions(px4iofirmware PRIVATE PX4IO_PERF) + target_link_libraries(px4iofirmware PRIVATE perf) +endif() diff --git a/src/prometheus_px4/src/modules/px4iofirmware/adc.c b/src/prometheus_px4/src/modules/px4iofirmware/adc.c new file mode 100644 index 0000000..1ac0fa2 --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/adc.c @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (c) 2012 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 adc.c + * + * Simple ADC support for PX4IO on STM32. + */ +#include +#include + +#include +#include +#include + +#include + +#if defined(PX4IO_PERF) +# include +#endif + +#define DEBUG +#include "px4io.h" + +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) + +#define rSR REG(STM32_ADC_SR_OFFSET) +#define rCR1 REG(STM32_ADC_CR1_OFFSET) +#define rCR2 REG(STM32_ADC_CR2_OFFSET) +#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET) +#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET) +#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET) +#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET) +#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET) +#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET) +#define rHTR REG(STM32_ADC_HTR_OFFSET) +#define rLTR REG(STM32_ADC_LTR_OFFSET) +#define rSQR1 REG(STM32_ADC_SQR1_OFFSET) +#define rSQR2 REG(STM32_ADC_SQR2_OFFSET) +#define rSQR3 REG(STM32_ADC_SQR3_OFFSET) +#define rJSQR REG(STM32_ADC_JSQR_OFFSET) +#define rJDR1 REG(STM32_ADC_JDR1_OFFSET) +#define rJDR2 REG(STM32_ADC_JDR2_OFFSET) +#define rJDR3 REG(STM32_ADC_JDR3_OFFSET) +#define rJDR4 REG(STM32_ADC_JDR4_OFFSET) +#define rDR REG(STM32_ADC_DR_OFFSET) + +#if defined(PX4IO_PERF) +perf_counter_t adc_perf; +#endif + +int +adc_init(void) +{ +#if defined(PX4IO_PERF) + adc_perf = perf_alloc(PC_ELAPSED, "adc"); +#endif + + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2 |= ADC_CR2_RSTCAL; + up_udelay(1); + + if (rCR2 & ADC_CR2_RSTCAL) { + return -1; + } + + rCR2 |= ADC_CR2_CAL; + up_udelay(100); + + if (rCR2 & ADC_CR2_CAL) { + return -1; + } + +#endif + + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; + rSMPR2 = 0b00011011011011011011011011011011; + + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 0; /* will be updated with the channel at conversion time */ + + return 0; +} + +/* + return one measurement, or 0xffff on error + */ +uint16_t +adc_measure(unsigned channel) +{ +#if defined(PX4IO_PERF) + perf_begin(adc_perf); +#endif + + /* clear any previous EOC */ + rSR = 0; + (void)rDR; + + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3 = channel; + rCR2 |= ADC_CR2_ADON; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + + while (!(rSR & ADC_SR_EOC)) { + + /* never spin forever - this will give a bogus result though */ + if (hrt_elapsed_time(&now) > 100) { +#if defined(PX4IO_PERF) + perf_end(adc_perf); +#endif + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + rSR = 0; + +#if defined(PX4IO_PERF) + perf_end(adc_perf); +#endif + return result; +} diff --git a/src/prometheus_px4/src/modules/px4iofirmware/controls.c b/src/prometheus_px4/src/modules/px4iofirmware/controls.c new file mode 100644 index 0000000..9e668de --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/controls.c @@ -0,0 +1,643 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file controls.c + * + * R/C inputs and servo outputs. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#if defined(PX4IO_PERF) +# include +#endif + +#include "px4io.h" + +#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ +#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ + +static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); +static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated); + +#if defined(PX4IO_PERF) +static perf_counter_t c_gather_dsm; +static perf_counter_t c_gather_sbus; +static perf_counter_t c_gather_ppm; +#endif + +static int _dsm_fd = -1; +int _sbus_fd = -1; + +static uint16_t rc_value_override = 0; + +#ifdef ADC_RSSI +static unsigned _rssi_adc_counts = 0; +#endif + +/* receive signal strenght indicator (RSSI). 0 = no connection, 100 (RC_INPUT_RSSI_MAX): perfect connection */ +/* Note: this is static because RC-provided telemetry does not occur every tick */ +static uint16_t _rssi = 0; +static unsigned _frame_drops = 0; + +bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) +{ +#if defined(PX4IO_PERF) + perf_begin(c_gather_dsm); +#endif + uint8_t n_bytes = 0; + uint8_t *bytes; + bool dsm_11_bit; + int8_t spektrum_rssi; + unsigned frame_drops; + *dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, + &spektrum_rssi, &frame_drops, PX4IO_RC_INPUT_CHANNELS); + + if (*dsm_updated) { + + if (dsm_11_bit) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + + } else { + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + } + + if (frame_drops == _frame_drops) { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + + } else { + r_raw_rc_flags |= (PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + _frame_drops = frame_drops; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + + if (spektrum_rssi >= 0 && spektrum_rssi <= 100) { + + /* ensure ADC RSSI is disabled */ + r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI); + + *rssi = spektrum_rssi; + } + } + +#if defined(PX4IO_PERF) + perf_end(c_gather_dsm); +#endif + + /* get data from FD and attempt to parse with DSM and ST24 libs */ + uint8_t st24_rssi, lost_count; + uint16_t st24_channel_count = 0; + + *st24_updated = false; + + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) { + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; + *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count, + &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + } + } + + if (*st24_updated && lost_count == 0) { + + /* ensure ADC RSSI is disabled */ + r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI); + + *rssi = st24_rssi; + r_raw_rc_count = st24_channel_count; + + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + + + /* get data from FD and attempt to parse with SUMD libs */ + uint8_t sumd_rssi, sumd_rx_count; + uint16_t sumd_channel_count = 0; + bool sumd_failsafe_state; + + *sumd_updated = false; + + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) { + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + sumd_rssi = RC_INPUT_RSSI_MAX; + *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, + &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state)); + } + } + + if (*sumd_updated) { + + /* not setting RSSI since SUMD does not provide one */ + r_raw_rc_count = sumd_channel_count; + + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + + if (sumd_failsafe_state) { + r_raw_rc_flags |= (PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } + + return (*dsm_updated | *st24_updated | *sumd_updated); +} + +void +controls_init(void) +{ + /* no channels */ + r_raw_rc_count = 0; + system_state.rc_channels_timestamp_received = 0; + system_state.rc_channels_timestamp_valid = 0; + + /* DSM input (USART1) */ + _dsm_fd = dsm_init("/dev/ttyS0"); + + /* S.bus input (USART3) */ + _sbus_fd = sbus_init("/dev/ttyS2", false); + + /* default to a 1:1 input map, all enabled */ + for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + +#if defined(PX4IO_PERF) + c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); + c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); + c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm"); +#endif +} + +void +controls_tick() +{ + + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ + +#ifdef ADC_RSSI + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { + unsigned counts = adc_measure(ADC_RSSI); + + if (counts != 0xffff) { + /* low pass*/ + _rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f); + /* use 1:1 scaling on 3.3V, 12-Bit ADC input */ + unsigned mV = _rssi_adc_counts * 3300 / 4095; + /* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */ + _rssi = (mV * RC_INPUT_RSSI_MAX / 3300); + + if (_rssi > RC_INPUT_RSSI_MAX) { + _rssi = RC_INPUT_RSSI_MAX; + } + } + } + +#endif + + /* zero RSSI if signal is lost */ + if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) { + _rssi = 0; + } + +#if defined(PX4IO_PERF) + perf_begin(c_gather_sbus); +#endif + + bool sbus_updated = false; + + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24 | PX4IO_P_STATUS_FLAGS_RC_SUMD))) { + bool sbus_failsafe, sbus_frame_drop; + sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + PX4IO_RC_INPUT_CHANNELS); + + if (sbus_updated) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); + + unsigned sbus_rssi = RC_INPUT_RSSI_MAX; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + sbus_rssi = RC_INPUT_RSSI_MAX / 2; + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + + /* set RSSI to an emulated value if ADC RSSI is off */ + if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + _rssi = sbus_rssi; + } + + } + } + +#if defined(PX4IO_PERF) + perf_end(c_gather_sbus); +#endif + + /* + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have S.bus signal. + */ +#if defined(PX4IO_PERF) + perf_begin(c_gather_ppm); +#endif + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); + + if (ppm_updated) { + + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_PPM); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + +#if defined(PX4IO_PERF) + perf_end(c_gather_ppm); +#endif + + bool dsm_updated = false, st24_updated = false, sumd_updated = false; + + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_SBUS | PX4IO_P_STATUS_FLAGS_RC_PPM))) { +#if defined(PX4IO_PERF) + perf_begin(c_gather_dsm); +#endif + + (void)dsm_port_input(&_rssi, &dsm_updated, &st24_updated, &sumd_updated); + + if (dsm_updated) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_DSM); + } + + if (st24_updated) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); + } + + if (sumd_updated) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); + } + +#if defined(PX4IO_PERF) + perf_end(c_gather_dsm); +#endif + } + + /* limit number of channels to allowable data size */ + if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) { + r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + } + + /* store RSSI */ + r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = _rssi; + + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; + + /* + * If we received a new frame from any of the RC sources, process it. + */ + if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) { + + /* record a bitmask of channels assigned */ + unsigned assigned_channels = 0; + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_received = hrt_absolute_time(); + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) { + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + } + + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) { + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + } + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( + conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( + conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) { + scaled = -scaled; + } + + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + + if (mapped < PX4IO_CONTROL_CHANNELS) { + + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) { + /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + } + + if (mapped == 3 && r_setup_rc_thr_failsafe) { + /* throttle failsafe detection */ + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + + } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) { + /* pick out override channel, indicated by special mapping */ + rc_value_override = SIGNED_TO_REG(scaled); + } + } + } + + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) { + r_rc_values[i] = 0; + } + } + + /* set RC OK flag, as we got an update */ + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK); + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK; + + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); + } + + /* + * Export the valid channel bitmap + */ + r_rc_valid = assigned_channels; + } + + /* + * If we haven't seen any new control data in 200ms, assume we + * have lost input. + */ + if (!rc_input_lost && hrt_elapsed_time_atomic(&system_state.rc_channels_timestamp_received) > 200000) { + rc_input_lost = true; + + /* clear the input-kind flags here */ + atomic_modify_clear(&r_status_flags, ( + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS | + PX4IO_P_STATUS_FLAGS_RC_ST24 | + PX4IO_P_STATUS_FLAGS_RC_SUMD)); + + } + + /* + * Handle losing RC input + */ + + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { + /* Clear the RC input status flag, clear manual override flag */ + atomic_modify_clear(&r_status_flags, ( + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK)); + + /* flag raw RC as lost */ + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); + + /* Mark all channels as invalid, as we just lost the RX */ + r_rc_valid = 0; + + /* Set raw channel count to zero */ + r_raw_rc_count = 0; + + /* Set the RC_LOST alarm */ + atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST); + } + + /* + * Check for manual override. + * + * Firstly, manual override must be enabled, RC input available and a mixer loaded. + */ + if (/* condition 1: Override is always allowed */ + (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && + /* condition 2: We have valid RC control inputs from the user */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + /* condition 3: The system didn't go already into failsafe mode with fixed outputs */ + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) && + /* condition 4: RC handling wasn't generally disabled */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + /* condition 5: We have a valid mixer to map RC inputs to actuator outputs */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + bool override = false; + + /* + * Check mapped channel 5 (can be any remote channel, + * depends on RC_MAP_OVER parameter); + * If the value is 'high' then the pilot has + * requested override. + * + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) { + override = true; + } + + /* + * If the FMU is dead then enable override if we have a mixer + * and we want to immediately override (instead of using the RC channel + * as in the case above. + * + * Also, do not enter manual override if we asked for termination + * failsafe and FMU is lost. + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { + override = true; + } + + if (override) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); + + } else { + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + + } else { + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); + } +} + +static bool +ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) +{ + bool result = false; + + if (!(num_values) || !(values) || !(frame_len)) { + return result; + } + + /* avoid racing with PPM updates */ + irqstate_t state = px4_enter_critical_section(); + + /* + * If we have received a new PPM frame within the last 200ms, accept it + * and then invalidate it. + */ + if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) { + + /* PPM data exists, copy it */ + *num_values = ppm_decoded_channels; + + if (*num_values > PX4IO_RC_INPUT_CHANNELS) { + *num_values = PX4IO_RC_INPUT_CHANNELS; + } + + for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { + values[i] = ppm_buffer[i]; + } + + /* clear validity */ + ppm_last_valid_decode = 0; + + /* store PPM frame length */ + *frame_len = ppm_frame_length; + + /* good if we got any channels */ + result = (*num_values > 0); + } + + px4_leave_critical_section(state); + + return result; +} diff --git a/src/prometheus_px4/src/modules/px4iofirmware/mixer.cpp b/src/prometheus_px4/src/modules/px4iofirmware/mixer.cpp new file mode 100644 index 0000000..6d6de62 --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/mixer.cpp @@ -0,0 +1,681 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mixer.cpp + * + * Control channel input/output mixer and failsafe. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +#include "mixer.h" + +extern "C" { + /* #define DEBUG */ +#include "px4io.h" +} + +/* + * Maximum interval in us before FMU signal is considered lost + */ +#define FMU_INPUT_DROP_LIMIT_US 500000 + +/* current servo arm/disarm state */ +static volatile bool mixer_servos_armed = false; +static volatile bool should_arm = false; +static volatile bool should_arm_nothrottle = false; +static volatile bool should_always_enable_pwm = false; +static volatile bool mix_failsafe = false; + +static bool new_fmu_data = false; +static uint64_t last_fmu_update = 0; + +extern int _sbus_fd; + +/* selected control values and count for mixing */ +enum mixer_source { + MIX_NONE, + MIX_DISARMED, + MIX_FMU, + MIX_OVERRIDE, + MIX_FAILSAFE, + MIX_OVERRIDE_FMU_OK +}; + +static volatile mixer_source source; + +static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control); +static int mixer_handle_text_create_mixer(); +static void mixer_mix_failsafe(); + +static MixerGroup mixer_group; + +void +mixer_tick() +{ + /* check if the mixer got modified */ + mixer_handle_text_create_mixer(); + + if (mix_failsafe) { + mixer_mix_failsafe(); + mix_failsafe = false; + } + + /* check that we are receiving fresh data from the FMU */ + irqstate_t irq_flags = enter_critical_section(); + const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time; + leave_critical_section(irq_flags); + + if ((fmu_data_received_time == 0) || + hrt_elapsed_time(&fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + + /* too long without FMU input, time to go to failsafe */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { + isr_debug(1, "AP RX timeout"); + } + + atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FMU_OK)); + atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_FMU_LOST); + + } else { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK); + + /* this flag is never cleared once OK */ + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED); + + if (fmu_data_received_time > last_fmu_update) { + new_fmu_data = true; + last_fmu_update = fmu_data_received_time; + } + } + + /* default to disarmed mixing */ + source = MIX_DISARMED; + + /* + * Decide which set of controls we're using. + */ + + /* Do not mix if we have raw PWM and FMU is ok. */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) > 0) { + /* a channel based override has been + * triggered, with FMU active */ + source = MIX_OVERRIDE_FMU_OK; + + } else { + /* don't actually mix anything - copy values from r_page_direct_pwm */ + source = MIX_NONE; + memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT); + } + + } else { + + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* mix from FMU controls */ + source = MIX_FMU; + } + + else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { + + if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { + + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; + + } else { + /* if allowed, mix from RC inputs directly */ + source = MIX_OVERRIDE; + } + } + } + + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + */ + should_arm = ( + (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ + && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */ + || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */ + ) + ); + + should_arm_nothrottle = ( + (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ + && (((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */ + || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */ + ) + ); + + should_always_enable_pwm = ( + (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) + ); + + /* + * Check if FMU is still alive, if not, terminate the flight + */ + if (REG_TO_BOOL(r_setup_flighttermination) && /* Flight termination is allowed */ + (source == MIX_DISARMED) && /* and if we ended up not changing the default mixer */ + should_arm && /* and we should be armed, so we intended to provide outputs */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { /* and FMU is initialized */ + atomic_modify_or(&r_setup_arming, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); /* then FMU is dead -> terminate flight */ + } + + /* + * Check if we should force failsafe - and do it if we have to + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { + source = MIX_FAILSAFE; + } + + /* + * Set failsafe status flag depending on mixing source + */ + if (source == MIX_FAILSAFE) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FAILSAFE); + + } else { + atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); + } + + /* + * Set simple mixer trim values. If the OK flag is set the mixer is fully loaded. + */ + if (update_trims && r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) { + update_trims = false; + mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT); + } + + /* + * Update air-mode parameter + */ + mixer_group.set_airmode((Mixer::Airmode)REG_TO_SIGNED(r_setup_airmode)); + + + /* + * Run the mixers. + */ + if (source == MIX_FAILSAFE) { + + /* copy failsafe values to the servo outputs */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_servos[i] = r_page_servo_failsafe[i]; + + /* safe actuators for FMU feedback */ + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); + } + + } else if (source == MIX_DISARMED) { + + /* copy disarmed values to the servo outputs */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_servos[i] = r_page_servo_disarmed[i]; + + /* safe actuators for FMU feedback */ + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); + } + + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) + && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { + + float outputs[PX4IO_SERVO_COUNT]; + unsigned mixed; + + if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) { + /* maximum value the outputs of the multirotor mixer are allowed to change in this cycle + * factor 2 is needed because actuator outputs are in the range [-1,1] + */ + float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT( + r_setup_slew_max); + mixer_group.set_max_delta_out_once(delta_out_max); + } + + /* set dt to be used in simple mixer for slew rate limiting */ + mixer_group.set_dt_once(dt); + + /* update parameter for mc thrust model if it updated */ + if (update_mc_thrust_param) { + mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac)); + update_mc_thrust_param = false; + } + + /* mix */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) { + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + r_mixer_limits = mixer_group.get_saturation_status(); + + } else { + mixed = 0; + } + + /* the pwm limit call takes care of out of band errors */ + output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, + r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + /* clamp unused outputs to zero */ + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { + r_page_servos[i] = 0; + outputs[i] = 0.0f; + } + + /* store normalized outputs */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + } + + + if (mixed && new_fmu_data) { + new_fmu_data = false; + + /* Trigger all timer's channels in Oneshot mode to fire + * the oneshots with updated values. + */ + + up_pwm_update(); + } + } + + /* set arming */ + bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm); + + /* lockdown means to send a valid pulse which disables the outputs */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { + needs_to_arm = true; + } + + if (needs_to_arm && !mixer_servos_armed) { + /* need to arm, but not armed */ + up_pwm_servo_arm(true, 0); + mixer_servos_armed = true; + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> PWM enabled"); + + } else if (!needs_to_arm && mixer_servos_armed) { + /* armed but need to disarm */ + up_pwm_servo_arm(false, 0); + mixer_servos_armed = false; + atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)); + isr_debug(5, "> PWM disabled"); + } + + if (mixer_servos_armed + && (should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE)) + && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { + /* update the servo outputs. */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + up_pwm_servo_set(i, r_page_servos[i]); + } + + /* set S.BUS1 or S.BUS2 outputs */ + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); + + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); + } + + } else if (mixer_servos_armed && (should_always_enable_pwm + || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) { + /* set the disarmed servo outputs. */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + up_pwm_servo_set(i, r_page_servo_disarmed[i]); + /* copy values into reporting register */ + r_page_servos[i] = r_page_servo_disarmed[i]; + } + + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); + } + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); + } + } +} + +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) +{ + control = 0.0f; + + if (control_group >= PX4IO_CONTROL_GROUPS) { + return -1; + } + + switch (source) { + case MIX_FMU: + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { + if (r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)] == INT16_MAX) { + //catch NAN values encoded as INT16 max for disarmed outputs + control = NAN; + + } else { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + } + + break; + } + + return -1; + + case MIX_OVERRIDE: + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } + + return -1; + + case MIX_OVERRIDE_FMU_OK: + + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + break; + } + + return -1; + + case MIX_DISARMED: + case MIX_FAILSAFE: + case MIX_NONE: + control = 0.0f; + return -1; + } + + /* apply trim offsets for override channels */ + if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_ROLL) { + control *= REG_TO_FLOAT(r_setup_scale_roll); + control += REG_TO_FLOAT(r_setup_trim_roll); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_PITCH) { + control *= REG_TO_FLOAT(r_setup_scale_pitch); + control += REG_TO_FLOAT(r_setup_trim_pitch); + + } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_YAW) { + control *= REG_TO_FLOAT(r_setup_scale_yaw); + control += REG_TO_FLOAT(r_setup_trim_yaw); + } + } + + /* limit output */ + control = math::constrain(control, -1.f, 1.f); + + /* motor spinup phase - lock throttle to zero */ + if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { + if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || + control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + control = 0.0f; + } + } + + /* only safety off, but not armed - set throttle as invalid */ + if (should_arm_nothrottle && !should_arm) { + if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || + control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* mark the throttle as invalid */ + control = NAN; + } + } + + return 0; +} + +/* + * XXX error handling here should be more aggressive; currently it is + * possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has + * not loaded faithfully. + */ + +static char mixer_text[PX4IO_MAX_MIXER_LENGTH]; /* large enough for one mixer */ +static unsigned mixer_text_length = 0; +static volatile bool mixer_update_pending = false; +static volatile bool mixer_reset_pending = false; + +int +mixer_handle_text_create_mixer() +{ + /* do not allow a mixer change while safety off and FMU armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + return 1; + } + + if (mixer_reset_pending) { + mixer_group.reset(); + mixer_reset_pending = false; + } + + /* only run on update */ + if (!mixer_update_pending || (mixer_text_length == 0)) { + return 0; + } + + /* process the text buffer, adding new mixers as their descriptions can be parsed */ + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid); + + /* if anything was parsed */ + if (resid != mixer_text_length) { + + isr_debug(2, "used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) { + memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + /* enforce null termination */ + mixer_text[resid] = '\0'; + } + + mixer_text_length = resid; + } + + mixer_update_pending = false; + + update_trims = true; + update_mc_thrust_param = true; + + return 0; +} + +int interrupt_mixer_handle_text(const void *buffer, size_t length) +{ + /* do not allow a mixer change while safety off and FMU armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + return 1; + } + + /* disable mixing, will be enabled once load is complete */ + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); + + px4io_mixdata *msg = (px4io_mixdata *)buffer; + + isr_debug(2, "mix txt %u", length); + + if (length < sizeof(px4io_mixdata)) { + return 0; + } + + unsigned text_length = length - sizeof(px4io_mixdata); + + switch (msg->action) { + case F2I_MIXER_ACTION_RESET: + isr_debug(2, "reset"); + + /* THEN actually delete it */ + mixer_reset_pending = true; + mixer_text_length = 0; + + /* FALLTHROUGH */ + case F2I_MIXER_ACTION_APPEND: + isr_debug(2, "append %d", length); + + /* check for overflow - this would be really fatal */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); + return 0; + } + + /* check if the last item has been processed - bail out if not */ + if (mixer_update_pending) { + return 1; + } + + /* append mixer text and nul-terminate, guard against overflow */ + memcpy(&mixer_text[mixer_text_length], msg->text, text_length); + mixer_text_length += text_length; + mixer_text[mixer_text_length] = '\0'; + isr_debug(2, "buflen %u", mixer_text_length); + + /* flag the buffer as ready */ + mixer_update_pending = true; + + break; + } + + return 0; +} + +void interrupt_mixer_set_failsafe() +{ + mix_failsafe = true; +} + +void +mixer_mix_failsafe() +{ + /* + * Check if a custom failsafe value has been written, + * or if the mixer is not ok and bail out. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + return; + } + + /* set failsafe defaults to the values for all inputs = 0 */ + float outputs[PX4IO_SERVO_COUNT]; + unsigned mixed; + + if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) { + /* maximum value the outputs of the multirotor mixer are allowed to change in this cycle + * factor 2 is needed because actuator outputs are in the range [-1,1] + */ + float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT( + r_setup_slew_max); + mixer_group.set_max_delta_out_once(delta_out_max); + } + + /* set dt to be used in simple mixer for slew rate limiting */ + mixer_group.set_dt_once(dt); + + /* update parameter for mc thrust model if it updated */ + if (update_mc_thrust_param) { + mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac)); + update_mc_thrust_param = false; + } + + /* mix */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) { + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + r_mixer_limits = mixer_group.get_saturation_status(); + + } else { + mixed = 0; + } + + /* scale to PWM and update the servo outputs as required */ + for (unsigned i = 0; i < mixed; i++) { + /* scale to servo output */ + r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500; + } + + /* disable the rest of the outputs */ + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { + r_page_servo_failsafe[i] = 0; + } +} diff --git a/src/prometheus_px4/src/modules/px4iofirmware/mixer.h b/src/prometheus_px4/src/modules/px4iofirmware/mixer.h new file mode 100644 index 0000000..ea1335c --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/mixer.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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 mixer.h + * + * PX4IO mixer definitions + * + * @author Lorenz Meier + */ +#pragma once +#define PX4IO_MAX_MIXER_LENGTH 330 diff --git a/src/prometheus_px4/src/modules/px4iofirmware/protocol.h b/src/prometheus_px4/src/modules/px4iofirmware/protocol.h new file mode 100644 index 0000000..3cfcee2 --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/protocol.h @@ -0,0 +1,405 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * @file protocol.h + * + * PX4IO interface protocol. + * + * @author Lorenz Meier + * + * Communication is performed via writes to and reads from 16-bit virtual + * registers organised into pages of 255 registers each. + * + * The first two bytes of each write select a page and offset address + * respectively. Subsequent reads and writes increment the offset within + * the page. + * + * Some pages are read- or write-only. + * + * Note that some pages may permit offset values greater than 255, which + * can only be achieved by long writes. The offset does not wrap. + * + * Writes to unimplemented registers are ignored. Reads from unimplemented + * registers return undefined values. + * + * As convention, values that would be floating point in other parts of + * the PX4 system are expressed as signed integer values scaled by 10000, + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. + * + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. + */ + +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f)) + +#define REG_TO_BOOL(_reg) ((bool)(_reg)) + +#define PX4IO_PROTOCOL_VERSION 4 + +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + +/* static configuration page */ +#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ +#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ +#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ +#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_MAX_TRANSFER_LEN 64 + +/* dynamic status page */ +#define PX4IO_PAGE_STATUS 1 +#define PX4IO_P_STATUS_FREEMEM 0 +#define PX4IO_P_STATUS_CPULOAD 1 + +#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */ + +#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ + +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ + +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ + +/* array of post-mix actuator outputs, -10000..10000 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of PWM servo output values, microseconds */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of raw RC input values, microseconds */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ +#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 50 +#define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ + +#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ +#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_RELAYS_PAD 5 + +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; +/* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +/* storage space of 12 occupied by CRC */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state - this is a non-data write and + hence index 12 can safely be used. */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ + +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ + +#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ +#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */ + +#define PX4IO_P_SETUP_SBUS_RATE 22 /**< frame rate of SBUS1 output in Hz */ + +#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */ + +#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */ + +#define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */ + +#define PX4IO_P_SETUP_AIRMODE 27 /**< air-mode */ + +#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 28 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */ + +#define PX4IO_THERMAL_IGNORE UINT16_MAX +#define PX4IO_THERMAL_OFF 0 +#define PX4IO_THERMAL_FULL 10000 + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 52 + +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ + +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM mtrim values for central position */ +#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +#pragma pack(push, 1) +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; +#pragma pack(pop) + +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#if (PX4IO_MAX_TRANSFER_LEN > PKT_MAX_REGS * 2) +#error The max transfer length of the IO protocol must not be larger than the IO packet size +#endif + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } + + return c; +} diff --git a/src/prometheus_px4/src/modules/px4iofirmware/px4io.c b/src/prometheus_px4/src/modules/px4iofirmware/px4io.c new file mode 100644 index 0000000..520a462 --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/px4io.c @@ -0,0 +1,491 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file px4io.c + * Top-level logic for the PX4IO module. + * + * @author Lorenz Meier + */ + +#include + +#include // required for task_create +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#if defined(PX4IO_PERF) +# include +#endif + +#include + +#include + +#define DEBUG +#include "px4io.h" + +__EXPORT int user_start(int argc, char *argv[]); + +struct sys_state_s system_state; + +static struct hrt_call serial_dma_call; + +output_limit_t pwm_limit; + +float dt; + +/* + * a set of debug buffers to allow us to send debug information from ISRs + */ + +static volatile uint32_t msg_counter; +static volatile uint32_t last_msg_counter; +static volatile uint8_t msg_next_out, msg_next_in; + +/* + * WARNING: too large buffers here consume the memory required + * for mixer handling. Do not allocate more than 80 bytes for + * output. + */ +#define NUM_MSG 1 +static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]; + +static void heartbeat_blink(void); +static void ring_blink(void); +static void update_mem_usage(void); + +void atomic_modify_or(volatile uint16_t *target, uint16_t modification) +{ + if ((*target | modification) != *target) { + PX4_CRITICAL_SECTION(*target |= modification); + } +} + +void atomic_modify_clear(volatile uint16_t *target, uint16_t modification) +{ + if ((*target & ~modification) != *target) { + PX4_CRITICAL_SECTION(*target &= ~modification); + } +} + +void atomic_modify_and(volatile uint16_t *target, uint16_t modification) +{ + if ((*target & modification) != *target) { + PX4_CRITICAL_SECTION(*target &= modification); + } +} + +/* + * add a debug message to be printed on the console + */ +void +isr_debug(uint8_t level, const char *fmt, ...) +{ + if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { + return; + } + + va_list ap; + va_start(ap, fmt); + vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); + va_end(ap); + msg_next_in = (msg_next_in + 1) % NUM_MSG; + msg_counter++; +} + +/* + * show all pending debug messages + */ +static void +show_debug_messages(void) +{ + if (msg_counter != last_msg_counter) { + uint32_t n = msg_counter - last_msg_counter; + + if (n > NUM_MSG) { n = NUM_MSG; } + + last_msg_counter = msg_counter; + + while (n--) { + debug("%s", msg[msg_next_out]); + msg_next_out = (msg_next_out + 1) % NUM_MSG; + } + } +} + +/* + * Get the memory usage at 2 Hz while not armed + */ +static void +update_mem_usage(void) +{ + if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + return; + } + + static uint64_t last_mem_time = 0; + uint64_t now = hrt_absolute_time(); + + if (now - last_mem_time > (500 * 1000)) { + struct mallinfo minfo = mallinfo(); + r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks; + last_mem_time = now; + } +} + +static void +heartbeat_blink(void) +{ + static bool heartbeat = false; + LED_BLUE(heartbeat = !heartbeat); +} + +static void +ring_blink(void) +{ +#ifdef GPIO_LED4 + + if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + LED_RING(1); + return; + } + + // XXX this led code does have + // intentionally a few magic numbers. + const unsigned max_brightness = 118; + + static unsigned counter = 0; + static unsigned brightness = max_brightness; + static unsigned brightness_counter = 0; + static unsigned on_counter = 0; + + if (brightness_counter < max_brightness) { + + bool on = ((on_counter * 100) / brightness_counter + 1) <= ((brightness * 100) / max_brightness + 1); + + // XXX once led is PWM driven, + // remove the ! in the line below + // to return to the proper breathe + // animation / pattern (currently inverted) + LED_RING(!on); + brightness_counter++; + + if (on) { + on_counter++; + } + + } else { + + if (counter >= 62) { + counter = 0; + } + + int n; + + if (counter < 32) { + n = counter; + + } else { + n = 62 - counter; + } + + brightness = (n * n) / 8; + brightness_counter = 0; + on_counter = 0; + counter++; + } + +#endif +} + +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} + +static void +calculate_fw_crc(void) +{ +#define APP_SIZE_MAX 0xf000 +#define APP_LOAD_ADDRESS 0x08001000 + // compute CRC of the current firmware + uint32_t sum = 0; + + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { + uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); + sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); + } + + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; + r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16; +} + +int +user_start(int argc, char *argv[]) +{ + /* configure the first 8 PWM outputs (i.e. all of them) */ + up_pwm_servo_init(0xff); + + /* reset all to zero */ + memset(&system_state, 0, sizeof(system_state)); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* calculate our fw CRC so FMU can decide if we need to update */ + calculate_fw_crc(); + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ +#ifdef CONFIG_ARCH_DMA + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif + + /* print some startup info */ + syslog(LOG_INFO, "\nPX4IO: starting\n"); + + /* default all the LEDs to off while we start */ + LED_AMBER(false); + LED_BLUE(false); + LED_SAFETY(false); +#ifdef GPIO_LED4 + LED_RING(false); +#endif + + /* turn on servo power (if supported) */ +#ifdef POWER_SERVO + POWER_SERVO(true); +#endif + + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif + + /* start the safety switch handler */ + safety_init(); + + /* initialise the control inputs */ + controls_init(); + + /* set up the ADC */ + adc_init(); + + /* start the FMU interface */ + interface_init(); + +#if defined(PX4IO_PERF) + /* add a performance counter for mixing */ + perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + + /* add a performance counter for controls */ + perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); + + /* and one for measuring the loop rate */ + perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); +#endif + + struct mallinfo minfo = mallinfo(); + r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk; + syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + + /* initialize PWM limit lib */ + output_limit_init(&pwm_limit); + + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 550) { + + syslog(LOG_ERR, "ERR: not enough MEM"); + bool phase = false; + + while (true) { + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + + } else { + LED_AMBER(false); + LED_BLUE(true); + } + + up_udelay(250000); + + phase = !phase; + } + } + + /* Start the failsafe led init */ + failsafe_led_init(); + + /* + * Run everything in a tight loop. + */ + + uint64_t last_debug_time = 0; + uint64_t last_heartbeat_time = 0; + uint64_t last_loop_time = 0; + + for (;;) { + dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f; + last_loop_time = hrt_absolute_time(); + + if (dt < 0.0001f) { + dt = 0.0001f; + + } else if (dt > 0.02f) { + dt = 0.02f; + } + +#if defined(PX4IO_PERF) + /* track the rate at which the loop is running */ + perf_count(loop_perf); + + /* kick the mixer */ + perf_begin(mixer_perf); +#endif + + mixer_tick(); + +#if defined(PX4IO_PERF) + perf_end(mixer_perf); + + /* kick the control inputs */ + perf_begin(controls_perf); +#endif + + controls_tick(); + +#if defined(PX4IO_PERF) + perf_end(controls_perf); +#endif + + /* some boards such as Pixhawk 2.1 made + the unfortunate choice to combine the blue led channel with + the IMU heater. We need a software hack to fix the hardware hack + by allowing to disable the LED / heater. + */ + if (r_page_setup[PX4IO_P_SETUP_THERMAL] == PX4IO_THERMAL_IGNORE) { + /* + blink blue LED at 4Hz in normal operation. When in + override blink 4x faster so the user can clearly see + that override is happening. This helps when + pre-flight testing the override system + */ + uint32_t heartbeat_period_us = 250 * 1000UL; + + if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { + heartbeat_period_us /= 4; + } + + if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } + + } else if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) { + /* switch resistive heater off */ + LED_BLUE(false); + + } else { + /* switch resistive heater hard on */ + LED_BLUE(true); + } + + update_mem_usage(); + + ring_blink(); + + check_reboot(); + + /* check for debug activity (default: none) */ + show_debug_messages(); + + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ + if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { + + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", + (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], + (unsigned)r_status_flags, + (unsigned)r_setup_arming, + (unsigned)r_setup_features, + (unsigned)mallinfo().mxordblk); + last_debug_time = hrt_absolute_time(); + } + } +} + diff --git a/src/prometheus_px4/src/modules/px4iofirmware/px4io.h b/src/prometheus_px4/src/modules/px4iofirmware/px4io.h new file mode 100644 index 0000000..7b61e99 --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/px4io.h @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file px4io.h + * + * General defines and structures for the PX4IO module firmware. + * + * @author Lorenz Meier + */ + +#pragma once + +#include + +#include +#include + +#include + +#include "protocol.h" + +#include + +/* + * Constants and limits. + */ +#define PX4IO_BL_VERSION 3 +#define PX4IO_SERVO_COUNT 8 +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_CONTROL_GROUPS 4 +#define PX4IO_RC_INPUT_CHANNELS 18 +#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ + +/* + * Debug logging + */ + +#ifdef DEBUG +# include +# define debug(fmt, args...) syslog(LOG_DEBUG,fmt "\n", ##args) +#else +# define debug(fmt, args...) do {} while(0) +#endif + +/* + * Registers. + */ +extern volatile uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */ +extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */ +extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */ +extern uint16_t r_page_direct_pwm[]; /* PX4IO_PAGE_DIRECT_PWM */ +extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */ +extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */ +extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */ + +extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ +extern uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ +extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ +extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ +extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ +extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ +extern int16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */ +extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ + +/* + * Register aliases. + * + * Handy aliases for registers that are widely used. + */ +#define r_status_flags r_page_status[PX4IO_P_STATUS_FLAGS] +#define r_status_alarms r_page_status[PX4IO_P_STATUS_ALARMS] + +#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] +#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] +#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] +#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) +#define r_mixer_limits r_page_status[PX4IO_P_STATUS_MIXER] + +#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] +#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] +#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] +#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] +#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] + +#define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] + +#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL] +#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH] +#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW] +#define r_setup_scale_roll r_page_setup[PX4IO_P_SETUP_SCALE_ROLL] +#define r_setup_scale_pitch r_page_setup[PX4IO_P_SETUP_SCALE_PITCH] +#define r_setup_scale_yaw r_page_setup[PX4IO_P_SETUP_SCALE_YAW] +#define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE] +#define r_setup_thr_fac r_page_setup[PX4IO_P_SETUP_THR_MDL_FAC] +#define r_setup_slew_max r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX] +#define r_setup_airmode r_page_setup[PX4IO_P_SETUP_AIRMODE] +#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] + +#define r_control_values (&r_page_controls[0]) + +/* + * System state structure. + */ +struct sys_state_s { + + volatile uint64_t rc_channels_timestamp_received; + volatile uint64_t rc_channels_timestamp_valid; + + /** + * Last FMU receive time, in microseconds since system boot + */ + volatile uint64_t fmu_data_received_time; + +}; + +extern struct sys_state_s system_state; +extern float dt; +extern bool update_mc_thrust_param; +extern bool update_trims; + +/* + * PWM limit structure + */ +extern output_limit_t pwm_limit; + +/* + * GPIO handling. + */ +/* HEX Cube Orange and Cube Yellow uses an inverted signal to control the IMU heater */ +#ifdef CONFIG_ARCH_BOARD_CUBEPILOT_IO_V2 +#define LED_BLUE(_s) px4_arch_gpiowrite(GPIO_LED1, (_s)) +#else +#define LED_BLUE(_s) px4_arch_gpiowrite(GPIO_LED1, !(_s)) +#endif +#define LED_AMBER(_s) px4_arch_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) px4_arch_gpiowrite(GPIO_LED3, !(_s)) +#define LED_RING(_s) px4_arch_gpiowrite(GPIO_LED4, (_s)) + + +# define PX4IO_RELAY_CHANNELS 0 +# define ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) + +# define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + +#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY) + +#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel) + +#define PX4_CRITICAL_SECTION(cmd) { irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); } + +void atomic_modify_or(volatile uint16_t *target, uint16_t modification); +void atomic_modify_clear(volatile uint16_t *target, uint16_t modification); +void atomic_modify_and(volatile uint16_t *target, uint16_t modification); + +/* + * Mixer + */ +extern void mixer_tick(void); +extern int interrupt_mixer_handle_text(const void *buffer, size_t length); +/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ +extern void interrupt_mixer_set_failsafe(void); + +/** + * Safety switch/LED. + */ +extern void safety_init(void); +extern void failsafe_led_init(void); + +/** + * FMU communications + */ +extern void interface_init(void); +extern void interface_tick(void); + +/** + * Register space + */ +extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); + +/** + * Sensors/misc inputs + */ +extern int adc_init(void); +extern uint16_t adc_measure(unsigned channel); + +/** + * R/C receiver handling. + * + * Input functions return true when they receive an update from the RC controller. + */ +extern void controls_init(void); +extern void controls_tick(void); + +/** global debug level for isr_debug() */ +extern volatile uint8_t debug_level; + +/** send a debug message to the console */ +extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); + diff --git a/src/prometheus_px4/src/modules/px4iofirmware/registers.c b/src/prometheus_px4/src/modules/px4iofirmware/registers.c new file mode 100644 index 0000000..75bc7be --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/registers.c @@ -0,0 +1,1095 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file registers.c + * + * Implementation of the PX4IO register space. + * + * @author Lorenz Meier + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "px4io.h" +#include "protocol.h" + +static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); +static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); + +bool update_mc_thrust_param; +bool update_trims; + +/** + * PAGE 0 + * + * Static configuration parameters. + */ +static const uint16_t r_page_config[] = { + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, + [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = PX4IO_BL_VERSION, + [PX4IO_P_CONFIG_MAX_TRANSFER] = PX4IO_MAX_TRANSFER_LEN, + [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, +}; + +/** + * PAGE 1 + * + * Status values. + */ +volatile uint16_t r_page_status[] = { + [PX4IO_P_STATUS_FREEMEM] = 0, + [PX4IO_P_STATUS_CPULOAD] = 0, + [PX4IO_P_STATUS_FLAGS] = 0, + [PX4IO_P_STATUS_ALARMS] = 0, + [PX4IO_P_STATUS_VSERVO] = 0, + [PX4IO_P_STATUS_VRSSI] = 0, + [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_MIXER] = 0, +}; + +/** + * PAGE 2 + * + * Post-mixed actuator values. + */ +uint16_t r_page_actuators[PX4IO_SERVO_COUNT]; + +/** + * PAGE 3 + * + * Servo PWM values + */ +uint16_t r_page_servos[PX4IO_SERVO_COUNT]; + +/** + * PAGE 4 + * + * Raw RC input + */ +uint16_t r_page_raw_rc_input[] = { + [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_FLAGS] = 0, + [PX4IO_P_RAW_RC_NRSSI] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, + [PX4IO_P_RAW_FRAME_COUNT] = 0, + [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, + [PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 +}; + +/** + * PAGE 5 + * + * Scaled/routed RC input + */ +uint16_t r_page_rc_input[] = { + [PX4IO_P_RC_VALID] = 0, + [PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 +}; + +/** + * Scratch page; used for registers that are constructed as-read. + * + * PAGE 6 Raw ADC input. + * PAGE 7 PWM rate maps. + */ +uint16_t r_page_scratch[32]; + +/** + * PAGE 8 + * + * RAW PWM values + */ +uint16_t r_page_direct_pwm[PX4IO_SERVO_COUNT]; + +/** + * PAGE 100 + * + * Setup registers + */ +volatile uint16_t r_page_setup[] = { + /* default to RSSI ADC functionality */ + [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, + [PX4IO_P_SETUP_ARMING] = (PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE), + [PX4IO_P_SETUP_PWM_RATES] = 0, + [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, + [PX4IO_P_SETUP_PWM_ALTRATE] = 200, + [PX4IO_P_SETUP_SBUS_RATE] = 72, + /* this is unused, but we will pad it for readability (the compiler pads it automatically) */ + [PX4IO_P_SETUP_RELAYS_PAD] = 0, +#ifdef ADC_VSERVO + [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, +#else + [PX4IO_P_SETUP_VBATT_SCALE] = 10000, +#endif + [PX4IO_P_SETUP_SET_DEBUG] = 0, + [PX4IO_P_SETUP_REBOOT_BL] = 0, + [PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0, + [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, + [PX4IO_P_SETUP_PWM_REVERSE] = 0, + [PX4IO_P_SETUP_TRIM_ROLL] = 0, + [PX4IO_P_SETUP_TRIM_PITCH] = 0, + [PX4IO_P_SETUP_TRIM_YAW] = 0, + [PX4IO_P_SETUP_SCALE_ROLL] = 10000, + [PX4IO_P_SETUP_SCALE_PITCH] = 10000, + [PX4IO_P_SETUP_SCALE_YAW] = 10000, + [PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0, + [PX4IO_P_SETUP_AIRMODE] = 0, + [PX4IO_P_SETUP_THR_MDL_FAC] = 0, + [PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE, + [PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0 +}; + +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) + +#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ + PX4IO_P_SETUP_ARMING_FMU_PREARMED | \ + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ + PX4IO_P_SETUP_ARMING_LOCKDOWN | \ + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) +#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) +#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) + +/** + * PAGE 101 + * + * Control values from the FMU. + */ +uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS]; + +/* + * PAGE 102 does not have a buffer. + */ + +/** + * PAGE 103 + * + * R/C channel input configuration. + */ +uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; + +/* valid options */ +#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) + +/* + * PAGE 104 uses r_page_servos. + */ + +/** + * PAGE 105 + * + * Failsafe servo PWM values + * + * Disable pulses as default. + */ +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX }; + +/** + * PAGE 108 + * + * trim values for center position + * + */ +int16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM }; + +/** + * PAGE 109 + * + * disarmed PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + +int +registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + + switch (page) { + + /* handle bulk controls input */ + case PX4IO_PAGE_CONTROLS: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_controls[offset] = *values; + + offset++; + num_values--; + values++; + } + + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; + + system_state.fmu_data_received_time = hrt_absolute_time(); + + break; + + /* handle raw PWM input */ + case PX4IO_PAGE_DIRECT_PWM: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + if (*values != PWM_IGNORE_THIS_CHANNEL) { + r_page_direct_pwm[offset] = *values; + } + + offset++; + num_values--; + values++; + } + + system_state.fmu_data_received_time = hrt_absolute_time(); + r_status_flags |= PX4IO_P_STATUS_FLAGS_RAW_PWM; + + /* Trigger all timer's channels in Oneshot mode to fire + * the oneshots with updated values. + */ + up_pwm_update(); + + break; + + /* handle setup for servo failsafe values */ + case PX4IO_PAGE_FAILSAFE_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; + + } else { + r_page_servo_failsafe[offset] = *values; + } + + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; + + offset++; + num_values--; + values++; + } + + break; + + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_HIGHEST_MIN) { + r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_control_min[offset] = PWM_LOWEST_MIN; + + } else { + r_page_servo_control_min[offset] = *values; + } + + offset++; + num_values--; + values++; + } + + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; + + } else if (*values < PWM_LOWEST_MAX) { + r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + + } else { + r_page_servo_control_max[offset] = *values; + } + + offset++; + num_values--; + values++; + } + + break; + + case PX4IO_PAGE_CONTROL_TRIM_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + r_page_servo_control_trim[offset] = *values; + + offset++; + num_values--; + values++; + } + + update_trims = true; + + break; + + case PX4IO_PAGE_DISARMED_PWM: { + /* flag for all outputs */ + bool all_disarmed_off = true; + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* 0 means disabling always PWM */ + r_page_servo_disarmed[offset] = 0; + + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; + all_disarmed_off = false; + + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; + all_disarmed_off = false; + + } else { + r_page_servo_disarmed[offset] = *values; + all_disarmed_off = false; + } + + offset++; + num_values--; + values++; + } + + if (all_disarmed_off) { + /* disable PWM output if disarmed */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + + } else { + /* enable PWM output always */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + } + } + break; + + /* handle text going to the mixer parser */ + case PX4IO_PAGE_MIXERLOAD: + /* do not change the mixer if FMU is armed and IO's safety is off + * this state defines an active system. This check is done in the + * text handling function. + */ + return interrupt_mixer_handle_text(values, num_values * sizeof(*values)); + + default: + + /* avoid offset wrap */ + if ((offset + num_values) > 255) { + num_values = 255 - offset; + } + + /* iterate individual registers, set each in turn */ + while (num_values--) { + if (registers_set_one(page, offset, *values)) { + return -1; + } + + offset++; + values++; + } + + break; + } + + return 0; +} + +static int +registers_set_one(uint8_t page, uint8_t offset, uint16_t value) +{ + switch (page) { + + case PX4IO_PAGE_STATUS: + switch (offset) { + case PX4IO_P_STATUS_ALARMS: + /* clear bits being written */ + r_status_alarms &= ~value; + break; + + case PX4IO_P_STATUS_FLAGS: + + /* + * Allow FMU override of arming state (to allow in-air restores), + * but only if the arming state is not in sync on the IO side. + */ + + if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + + } else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + r_status_flags = value; + + } + + if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) { + /* update failsafe values, now that the mixer is set to ok */ + interrupt_mixer_set_failsafe(); + } + + break; + + default: + /* just ignore writes to other registers in this page */ + break; + } + + break; + + case PX4IO_PAGE_SETUP: + switch (offset) { + case PX4IO_P_SETUP_FEATURES: + + value &= PX4IO_P_SETUP_FEATURES_VALID; + + /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ + + /* switch S.Bus output pin as needed */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); + + /* disable the conflicting options with SBUS 1 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with SBUS 2 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } + +#endif + + /* disable the conflicting options with ADC RSSI */ + if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ + if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* apply changes */ + r_setup_features = value; + + break; + + case PX4IO_P_SETUP_ARMING: + + value &= PX4IO_P_SETUP_ARMING_VALID; + + /* + * Update arming state - disarm if no longer OK. + * This builds on the requirement that the FMU driver + * asks about the FMU arming state on initialization, + * so that an in-air reset of FMU can not lead to a + * lockup of the IO arming state. + */ + + if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + } + + /* + * If the failsafe termination flag is set, do not allow the autopilot to unset it + */ + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + + /* + * If failsafe termination is enabled and force failsafe bit is set, do not allow + * the autopilot to clear it. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) { + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + + r_setup_arming = value; + + break; + + case PX4IO_P_SETUP_PWM_RATES: + value &= PX4IO_P_SETUP_RATES_VALID; + pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); + break; + + case PX4IO_P_SETUP_PWM_DEFAULTRATE: + if (value < 25) { + value = 25; + } + + if (value > 400) { + value = 400; + } + + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); + break; + + case PX4IO_P_SETUP_PWM_ALTRATE: + + /* For PWM constrain to [25,400]Hz + * For Oneshot there is no rate, 0 is therefore used to select Oneshot mode + */ + if (value != 0) { + if (value < 25) { + value = 25; + } + + if (value > 400) { + value = 400; + } + } + + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); + break; + + case PX4IO_P_SETUP_SET_DEBUG: + r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; + isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); + break; + + case PX4IO_P_SETUP_REBOOT_BL: + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + // don't allow reboot while armed + break; + } + + // check the magic value + if (value != PX4IO_REBOOT_BL_MAGIC) { + break; + } + + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); + break; + + case PX4IO_P_SETUP_DSM: + dsm_bind(value & 0x0f, (value >> 4) & 0xF); + break; + + case PX4IO_P_SETUP_FORCE_SAFETY_ON: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + + } else { + return -1; + } + + break; + + case PX4IO_P_SETUP_FORCE_SAFETY_OFF: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + + } else { + return -1; + } + + break; + + case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: + if (value > 650 && value < 2350) { + r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; + } + + break; + + case PX4IO_P_SETUP_SBUS_RATE: + r_page_setup[offset] = value; + sbus1_set_output_rate_hz(value); + break; + + case PX4IO_P_SETUP_THR_MDL_FAC: + update_mc_thrust_param = true; + r_page_setup[offset] = value; + break; + + case PX4IO_P_SETUP_PWM_REVERSE: + case PX4IO_P_SETUP_TRIM_ROLL: + case PX4IO_P_SETUP_TRIM_PITCH: + case PX4IO_P_SETUP_TRIM_YAW: + case PX4IO_P_SETUP_SCALE_ROLL: + case PX4IO_P_SETUP_SCALE_PITCH: + case PX4IO_P_SETUP_SCALE_YAW: + case PX4IO_P_SETUP_MOTOR_SLEW_MAX: + case PX4IO_P_SETUP_AIRMODE: + case PX4IO_P_SETUP_THERMAL: + case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION: + r_page_setup[offset] = value; + break; + + default: + return -1; + } + + break; + + case PX4IO_PAGE_RC_CONFIG: { + + /** + * do not allow a RC config change while safety is off + */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + break; + } + + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; + + if (channel >= PX4IO_RC_INPUT_CHANNELS) { + return -1; + } + + /* disable the channel until we have a chance to sanity-check it */ + conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + switch (index) { + + case PX4IO_P_RC_CONFIG_MIN: + case PX4IO_P_RC_CONFIG_CENTER: + case PX4IO_P_RC_CONFIG_MAX: + case PX4IO_P_RC_CONFIG_DEADZONE: + case PX4IO_P_RC_CONFIG_ASSIGNMENT: + conf[index] = value; + break; + + case PX4IO_P_RC_CONFIG_OPTIONS: + value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + + /* set all options except the enabled option */ + conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + /* should the channel be enabled? */ + /* this option is normally set last */ + if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + bool disabled = false; + + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { + disabled = true; + + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { + count++; + } + + /* sanity checks pass, enable channel */ + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + + } else if (!disabled) { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + } + + break; + /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + + } + + break; + /* case PX4IO_RC_PAGE_CONFIG */ + } + + case PX4IO_PAGE_TEST: + switch (offset) { + case PX4IO_P_TEST_LED: + LED_AMBER(value & 1); + break; + } + + break; + + default: + return -1; + } + + return 0; +} + +uint8_t last_page; +uint8_t last_offset; + +int +registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) +{ +#define SELECT_PAGE(_page_name) \ + do { \ + *values = (uint16_t *)&_page_name[0]; \ + *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ + } while(0) + + switch (page) { + + /* + * Handle pages that are updated dynamically at read time. + */ + case PX4IO_PAGE_STATUS: + /* PX4IO_P_STATUS_FREEMEM */ + + /* XXX PX4IO_P_STATUS_CPULOAD */ + + /* PX4IO_P_STATUS_FLAGS maintained externally */ + + /* PX4IO_P_STATUS_ALARMS maintained externally */ + +#ifdef ADC_VBATT + /* PX4IO_P_STATUS_VBATT */ + { + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit, validated on sample points of another unit + * + * Data in Tools/tests-host/data folder. + * + * measured slope = 0.004585267878277 (int: 4585) + * nominal theoretic slope: 0.00459340659 (int: 4593) + * intercept = 0.016646394188076 (int: 16646) + * nominal theoretic intercept: 0.00 (int: 0) + * + */ + unsigned counts = adc_measure(ADC_VBATT); + + if (counts != 0xffff) { + unsigned mV = (166460 + (counts * 45934)) / 10000; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; + + r_page_status[PX4IO_P_STATUS_VBATT] = corrected; + } + } + +#endif +#ifdef ADC_IBATT + /* PX4IO_P_STATUS_IBATT */ + { + /* + note that we have no idea what sort of + current sensor is attached, so we just + return the raw 12 bit ADC value and let the + FMU sort it out, with user selectable + configuration for their sensor + */ + unsigned counts = adc_measure(ADC_IBATT); + + if (counts != 0xffff) { + r_page_status[PX4IO_P_STATUS_IBATT] = counts; + } + } +#endif +#ifdef ADC_VSERVO + /* PX4IO_P_STATUS_VSERVO */ + { + unsigned counts = adc_measure(ADC_VSERVO); + + if (counts != 0xffff) { + // use 3:1 scaling on 3.3V ADC input + unsigned mV = counts * 9900 / 4096; + r_page_status[PX4IO_P_STATUS_VSERVO] = mV; + } + } +#endif +#ifdef ADC_RSSI + /* PX4IO_P_STATUS_VRSSI */ + { + unsigned counts = adc_measure(ADC_RSSI); + + if (counts != 0xffff) { + // use 1:1 scaling on 3.3V ADC input + unsigned mV = counts * 3300 / 4096; + r_page_status[PX4IO_P_STATUS_VRSSI] = mV; + } + } +#endif + /* XXX PX4IO_P_STATUS_PRSSI */ + + SELECT_PAGE(r_page_status); + break; + + case PX4IO_PAGE_RAW_ADC_INPUT: + memset(r_page_scratch, 0, sizeof(r_page_scratch)); +#ifdef ADC_VBATT + r_page_scratch[0] = adc_measure(ADC_VBATT); +#endif +#ifdef ADC_IBATT + r_page_scratch[1] = adc_measure(ADC_IBATT); +#endif + +#ifdef ADC_VSERVO + r_page_scratch[0] = adc_measure(ADC_VSERVO); +#endif +#ifdef ADC_RSSI + r_page_scratch[1] = adc_measure(ADC_RSSI); +#endif + SELECT_PAGE(r_page_scratch); + break; + + case PX4IO_PAGE_PWM_INFO: + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + } + + SELECT_PAGE(r_page_scratch); + break; + + /* + * Pages that are just a straight read of the register state. + */ + + /* status pages */ + case PX4IO_PAGE_CONFIG: + SELECT_PAGE(r_page_config); + break; + + case PX4IO_PAGE_ACTUATORS: + SELECT_PAGE(r_page_actuators); + break; + + case PX4IO_PAGE_SERVOS: + SELECT_PAGE(r_page_servos); + break; + + case PX4IO_PAGE_RAW_RC_INPUT: + SELECT_PAGE(r_page_raw_rc_input); + break; + + case PX4IO_PAGE_RC_INPUT: + SELECT_PAGE(r_page_rc_input); + break; + + /* readback of input pages */ + case PX4IO_PAGE_SETUP: + SELECT_PAGE(r_page_setup); + break; + + case PX4IO_PAGE_CONTROLS: + SELECT_PAGE(r_page_controls); + break; + + case PX4IO_PAGE_RC_CONFIG: + SELECT_PAGE(r_page_rc_input_config); + break; + + case PX4IO_PAGE_DIRECT_PWM: + SELECT_PAGE(r_page_direct_pwm); + break; + + case PX4IO_PAGE_FAILSAFE_PWM: + SELECT_PAGE(r_page_servo_failsafe); + break; + + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; + + case PX4IO_PAGE_CONTROL_TRIM_PWM: + SELECT_PAGE(r_page_servo_control_trim); + break; + + case PX4IO_PAGE_DISARMED_PWM: + SELECT_PAGE(r_page_servo_disarmed); + break; + + default: + return -1; + } + +#undef SELECT_PAGE +#undef COPY_PAGE + + last_page = page; + last_offset = offset; + + /* if the offset is at or beyond the end of the page, we have no data */ + if (offset >= *num_values) { + return -1; + } + + /* correct the data pointer and count for the offset */ + *values += offset; + *num_values -= offset; + + return 0; +} + +/* + * Helper function to handle changes to the PWM rate control registers. + */ +static void +pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) +{ + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { + + /* get the channel mask for this rate group */ + uint32_t mask = up_pwm_servo_get_rate_group(group); + + if (mask == 0) { + continue; + } + + /* all channels in the group must be either default or alt-rate */ + uint32_t alt = map & mask; + + if (pass == 0) { + /* preflight */ + if ((alt != 0) && (alt != mask)) { + /* not a legal map, bail with an alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + return; + } + + } else { + /* set it - errors here are unexpected */ + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) { + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + + } else { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) { + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } + } + } + } + + r_setup_pwm_rates = map; + r_setup_pwm_defaultrate = defaultrate; + r_setup_pwm_altrate = altrate; +} diff --git a/src/prometheus_px4/src/modules/px4iofirmware/safety.c b/src/prometheus_px4/src/modules/px4iofirmware/safety.c new file mode 100644 index 0000000..a28e680 --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/safety.c @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file safety.c + * Safety button logic. + * + * @author Lorenz Meier + */ + +#include + +#include + +#include + +#include "px4io.h" + +static struct hrt_call arming_call; +static struct hrt_call failsafe_call; + +/* + * Count the number of times in a row that we see the arming button + * held down. + */ +static unsigned counter = 0; + +/* + * Define the various LED flash sequences for each system state. + */ +#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */ +#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */ +#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */ +#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */ +#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */ + +static unsigned blink_counter = 0; + +/* + * IMPORTANT: The arming state machine critically + * depends on using the same threshold + * for arming and disarming. Since disarming + * is quite deadly for the system, a similar + * length can be justified. + */ +#define ARM_COUNTER_THRESHOLD 10 + +static void safety_check_button(void *arg); +static void failsafe_blink(void *arg); + +void +safety_init(void) +{ + /* arrange for the button handler to be called at 10Hz */ + hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} + +void +failsafe_led_init(void) +{ + /* arrange for the failsafe blinker to be called at 8Hz */ + hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); +} + +static void +safety_check_button(void *arg) +{ + const bool safety_button_pressed = BUTTON_SAFETY; + + /* Keep safety button pressed for one second to turn off safety + * + * Note that safety cannot be turned on again by button because a button + * hardware problem could accidentally disable it in flight. + */ + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { + + if (counter <= ARM_COUNTER_THRESHOLD) { + counter++; + + } + + if (counter == ARM_COUNTER_THRESHOLD) { + // switch safety off -> ready to arm state + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF); + } + + } else { + counter = 0; + } + + /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ + uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; + + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { + pattern = LED_PATTERN_IO_FMU_ARMED; + + } else { + pattern = LED_PATTERN_IO_ARMED; + } + + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { + pattern = LED_PATTERN_FMU_ARMED; + + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) { + pattern = LED_PATTERN_FMU_OK_TO_ARM; + + } + + /* Turn the LED on if we have a 1 at the current bit position */ + LED_SAFETY(pattern & (1 << blink_counter++)); + + if (blink_counter > 15) { + blink_counter = 0; + } +} + +static void +failsafe_blink(void *arg) +{ + /* indicate that a serious initialisation error occured */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + LED_AMBER(true); + return; + } + + static bool failsafe = false; + + /* blink the failsafe LED if we don't have FMU input */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + failsafe = !failsafe; + + } else { + failsafe = false; + } + + LED_AMBER(failsafe); +} diff --git a/src/prometheus_px4/src/modules/px4iofirmware/serial.c b/src/prometheus_px4/src/modules/px4iofirmware/serial.c new file mode 100644 index 0000000..55bcfb6 --- /dev/null +++ b/src/prometheus_px4/src/modules/px4iofirmware/serial.c @@ -0,0 +1,382 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 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 serial.c + * + * Serial communication for the PX4IO module. + */ + +#include +#include +#include +#include +#include + +#include +#include + +/* XXX might be able to prune these */ +#include +#include +#include +#include + +//#define DEBUG +#include "px4io.h" + +#if defined(PX4IO_PERF) +# include + +static perf_counter_t pc_txns; +static perf_counter_t pc_errors; +static perf_counter_t pc_ore; +static perf_counter_t pc_fe; +static perf_counter_t pc_ne; +static perf_counter_t pc_idle; +static perf_counter_t pc_badidle; +static perf_counter_t pc_regerr; +static perf_counter_t pc_crcerr; +#endif + +static void rx_handle_packet(void); +static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static DMA_HANDLE tx_dma; +static DMA_HANDLE rx_dma; + +static int serial_interrupt(int irq, void *context, FAR void *arg); +static void dma_reset(void); + +static struct IOPacket dma_packet; + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + +void +interface_init(void) +{ +#if defined(PX4IO_PERF) + pc_txns = perf_alloc(PC_ELAPSED, "txns"); + pc_errors = perf_alloc(PC_COUNT, "errors"); + pc_ore = perf_alloc(PC_COUNT, "overrun"); + pc_fe = perf_alloc(PC_COUNT, "framing"); + pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_idle = perf_alloc(PC_COUNT, "idle"); + pc_badidle = perf_alloc(PC_COUNT, "badidle"); + pc_regerr = perf_alloc(PC_COUNT, "regerr"); + pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); +#endif + + /* allocate DMA */ + tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); + rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); + + /* configure pins for serial use */ + px4_arch_configgpio(PX4FMU_SERIAL_TX_GPIO); + px4_arch_configgpio(PX4FMU_SERIAL_RX_GPIO); + + /* reset and configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* clear status/errors */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* connect our interrupt */ + irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt, NULL); + up_enable_irq(PX4FMU_SERIAL_VECTOR); + + /* enable UART and error/idle interrupts */ + rCR3 = USART_CR3_EIE; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + +#if 0 /* keep this for signal integrity testing */ + + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + + rDR = 0xfa; + + while (!(rSR & USART_SR_TXE)) + ; + + rDR = 0xa0; + } + +#endif + + /* configure RX DMA and return to listening state */ + dma_reset(); + + debug("serial init"); +} + +static void +rx_handle_packet(void) +{ + /* check packet CRC */ + uint8_t crc = dma_packet.crc; + dma_packet.crc = 0; + + if (crc != crc_packet(&dma_packet)) { +#if defined(PX4IO_PERF) + perf_count(pc_crcerr); +#endif + + /* send a CRC error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xff; + + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { + + /* it's a blind write - pass it on */ + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { +#if defined(PX4IO_PERF) + perf_count(pc_regerr); +#endif + + dma_packet.count_code = PKT_CODE_ERROR; + + } else { + dma_packet.count_code = PKT_CODE_SUCCESS; + } + + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_READ) { + + /* it's a read - get register pointer for reply */ + unsigned count; + uint16_t *registers; + + if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { +#if defined(PX4IO_PERF) + perf_count(pc_regerr); +#endif + + dma_packet.count_code = PKT_CODE_ERROR; + + } else { + /* constrain reply to requested size */ + if (count > PKT_MAX_REGS) { + count = PKT_MAX_REGS; + } + + if (count > PKT_COUNT(dma_packet)) { + count = PKT_COUNT(dma_packet); + } + + /* copy reply registers into DMA buffer */ + memcpy((void *)&dma_packet.regs[0], registers, count * 2); + dma_packet.count_code = count | PKT_CODE_SUCCESS; + } + + return; + } + + /* send a bad-packet error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xfe; +} + +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + /* + * We are here because DMA completed, or UART reception stopped and + * we think we have a packet in the buffer. + */ +#if defined(PX4IO_PERF) + perf_begin(pc_txns); +#endif + + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* handle the received packet */ + rx_handle_packet(); + + /* re-set DMA for reception first, so we are ready to receive before we start sending */ + dma_reset(); + + /* send the reply to the just-processed request */ + dma_packet.crc = 0; + dma_packet.crc = crc_packet(&dma_packet); + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + PKT_SIZE(dma_packet), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + stm32_dmastart(tx_dma, NULL, NULL, false); + rCR3 |= USART_CR3_DMAT; + +#if defined(PX4IO_PERF) + perf_end(pc_txns); +#endif +} + +static int +serial_interrupt(int irq, void *context, FAR void *arg) +{ + static bool abort_on_idle = false; + + uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* required to clear any of the interrupt status that brought us here */ + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + +#if defined(PX4IO_PERF) + perf_count(pc_errors); + + if (sr & USART_SR_ORE) { + perf_count(pc_ore); + } + + if (sr & USART_SR_NE) { + perf_count(pc_ne); + } + + if (sr & USART_SR_FE) { + perf_count(pc_fe); + } + +#endif + + /* send a line break - this will abort transmission/reception on the other end */ + rCR1 |= USART_CR1_SBK; + + /* when the line goes idle, abort rather than look at the packet */ + abort_on_idle = true; + } + + if (sr & USART_SR_IDLE) { + + /* + * If we saw an error, don't bother looking at the packet - it should have + * been aborted by the sender and will definitely be bad. Get the DMA reconfigured + * ready for their retry. + */ + if (abort_on_idle) { + + abort_on_idle = false; + dma_reset(); + return 0; + } + + /* + * The sender has stopped sending - this is probably the end of a packet. + * Check the received length against the length in the header to see if + * we have something that looks like a packet. + */ + unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + + /* it was too short - possibly truncated */ +#if defined(PX4IO_PERF) + perf_count(pc_badidle); +#endif + dma_reset(); + return 0; + } + + /* + * Looks like we received a packet. Stop the DMA and go process the + * packet. + */ +#if defined(PX4IO_PERF) + perf_count(pc_idle); +#endif + stm32_dmastop(rx_dma); + rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); + } + + return 0; +} + +static void +dma_reset(void) +{ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* kill any pending DMA */ + stm32_dmastop(tx_dma); + stm32_dmastop(rx_dma); + + /* reset the RX side */ + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIVERYHI); + + /* start receive DMA ready for the next packet */ + stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAR; +} + diff --git a/src/prometheus_px4/src/modules/rc_update/CMakeLists.txt b/src/prometheus_px4/src/modules/rc_update/CMakeLists.txt new file mode 100644 index 0000000..c6ba78d --- /dev/null +++ b/src/prometheus_px4/src/modules/rc_update/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015-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__rc_update + MAIN rc_update + SRCS + rc_update.cpp + rc_update.h + DEPENDS + mathlib + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/rc_update/params.c b/src/prometheus_px4/src/modules/rc_update/params.c new file mode 100644 index 0000000..ed14dea --- /dev/null +++ b/src/prometheus_px4/src/modules/rc_update/params.c @@ -0,0 +1,2251 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 rc_params.c + * + * Parameters defined for RC. + * + */ + +/** + * RC channel 1 minimum + * + * Minimum value for RC channel 1 + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); + +/** + * RC channel 1 trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); + +/** + * RC channel 1 maximum + * + * Maximum value for RC channel 1 + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); + +/** + * RC channel 1 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); + +/** + * RC channel 1 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); + +/** + * RC channel 2 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); + +/** + * RC channel 2 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); + +/** + * RC channel 2 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); + +/** + * RC channel 2 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); + +/** + * RC channel 2 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); + +/** + * RC channel 3 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_MIN, 1000); + +/** + * RC channel 3 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); + +/** + * RC channel 3 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_MAX, 2000); + +/** + * RC channel 3 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); + +/** + * RC channel 3 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); + +/** + * RC channel 4 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_MIN, 1000); + +/** + * RC channel 4 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); + +/** + * RC channel 4 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_MAX, 2000); + +/** + * RC channel 4 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); + +/** + * RC channel 4 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); + +/** + * RC channel 5 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_MIN, 1000); + +/** + * RC channel 5 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); + +/** + * RC channel 5 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_MAX, 2000); + +/** + * RC channel 5 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); + +/** + * RC channel 5 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); + +/** + * RC channel 6 minimum + * + * Minimum value for this channel. + * + * @unit us + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_MIN, 1000); + +/** + * RC channel 6 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); + +/** + * RC channel 6 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_MAX, 2000); + +/** + * RC channel 6 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); + +/** + * RC channel 6 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); + +/** + * RC channel 7 minimum + * + * Minimum value for this channel. + * + * @unit us + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_MIN, 1000); + +/** + * RC channel 7 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); + +/** + * RC channel 7 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_MAX, 2000); + +/** + * RC channel 7 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); + +/** + * RC channel 7 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); + +/** + * RC channel 8 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_MIN, 1000); + +/** + * RC channel 8 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); + +/** + * RC channel 8 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_MAX, 2000); + +/** + * RC channel 8 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); + +/** + * RC channel 8 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); + +/** + * RC channel 9 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_MIN, 1000); + +/** + * RC channel 9 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); + +/** + * RC channel 9 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_MAX, 2000); + +/** + * RC channel 9 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); + +/** + * RC channel 9 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); + +/** + * RC channel 10 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_MIN, 1000); + +/** + * RC channel 10 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); + +/** + * RC channel 10 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_MAX, 2000); + +/** + * RC channel 10 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); + +/** + * RC channel 10 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); + +/** + * RC channel 11 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_MIN, 1000); + +/** + * RC channel 11 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); + +/** + * RC channel 11 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_MAX, 2000); + +/** + * RC channel 11 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); + +/** + * RC channel 11 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); + +/** + * RC channel 12 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_MIN, 1000); + +/** + * RC channel 12 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); + +/** + * RC channel 12 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_MAX, 2000); + +/** + * RC channel 12 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); + +/** + * RC channel 12 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); + +/** + * RC channel 13 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_MIN, 1000); + +/** + * RC channel 13 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); + +/** + * RC channel 13 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_MAX, 2000); + +/** + * RC channel 13 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); + +/** + * RC channel 13 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); + +/** + * RC channel 14 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_MIN, 1000); + +/** + * RC channel 14 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); + +/** + * RC channel 14 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_MAX, 2000); + +/** + * RC channel 14 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); + +/** + * RC channel 14 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); + +/** + * RC channel 15 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_MIN, 1000); + +/** + * RC channel 15 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); + +/** + * RC channel 15 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); + +/** + * RC channel 15 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); + +/** + * RC channel 15 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + +/** + * RC channel 16 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC16_MIN, 1000); + +/** + * RC channel 16 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC16_TRIM, 1500); + +/** + * RC channel 16 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC16_MAX, 2000); + +/** + * RC channel 16 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC16_REV, 1.0f); + +/** + * RC channel 16 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f); + +/** + * RC channel 17 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC17_MIN, 1000); + +/** + * RC channel 17 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC17_TRIM, 1500); + +/** + * RC channel 17 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC17_MAX, 2000); + +/** + * RC channel 17 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC17_REV, 1.0f); + +/** + * RC channel 17 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f); + +/** + * RC channel 18 minimum + * + * Minimum value for this channel. + * + * @min 800.0 + * @max 1500.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC18_MIN, 1000); + +/** + * RC channel 18 trim + * + * Mid point value (has to be set to the same as min for throttle channel). + * + * @min 800.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC18_TRIM, 1500); + +/** + * RC channel 18 maximum + * + * Maximum value for this channel. + * + * @min 1500.0 + * @max 2200.0 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC18_MAX, 2000); + +/** + * RC channel 18 reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); + +/** + * RC channel 18 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); + +/** + * RC channel count + * + * This parameter is used by Ground Station software to save the number + * of channels which were used during RC calibration. It is only meant + * for ground station use. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_CHAN_CNT, 0); + +/** + * Roll control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading roll inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_ROLL, 0); + +/** + * Pitch control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading pitch inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PITCH, 0); + +/** + * Failsafe channel mapping. + * + * Configures which channel is used by the receiver to indicate the signal was lost. + * Futaba receivers do report that way. + * If 0, whichever channel is mapped to throttle is used + * otherwise the value indicates the specific RC channel to use + * + * Use RC_FAILS_THR to set the threshold indicating lost signal. By default it's below + * the expected range and hence diabled. + * + * @min 0 + * @max 18 + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); + +/** + * Throttle control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading throttle inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0); + +/** + * Yaw control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading yaw inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_YAW, 0); + +/** + * Single channel flight mode selection + * + * If this parameter is non-zero, flight modes are only selected + * by this channel and are assigned to six slots. + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_FLTMODE, 0); + +/** + * Mode switch channel mapping. + * + * This is the main flight mode selector. + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for deciding about the main mode. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); + +/** + * Return switch channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); + +/** + * Position Control switch channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); + +/** + * Loiter switch channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); + +/** + * Acro switch channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); + +/** + * Offboard switch channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); + +/** + * Emergency Kill switch channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0); + +/** + * Arm switch channel. + * + * Use it to arm/disarm via switch instead of default throttle stick. If this is + * assigned, arming and disarming via stick is disabled. + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_ARM_SW, 0); + +/** + * Flaps channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); + +/** + * VTOL transition switch channel mapping + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0); + +/** + * Landing gear switch channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0); + +/** + * Stabilize switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0); + +/** + * Manual switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); + +/** + * AUX1 Passthrough RC channel + * + * Default function: Camera pitch + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); + +/** + * AUX2 Passthrough RC channel + * + * Default function: Camera roll + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); + +/** + * AUX3 Passthrough RC channel + * + * Default function: Camera azimuth / yaw + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); + +/** + * AUX4 Passthrough RC channel + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); + +/** + * AUX5 Passthrough RC channel + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); + +/** + * AUX6 Passthrough RC channel + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_AUX6, 0); +/** + * PARAM1 tuning channel + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 1st parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM1, 0); + +/** + * PARAM2 tuning channel + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 2nd parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM2, 0); + +/** + * PARAM3 tuning channel + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 3th parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0); + +/** + * Failsafe channel PWM threshold. + * + * Set to a value slightly above the PWM value assumed by throttle in a failsafe event, + * but ensure it is below the PWM value assumed by throttle during normal operation. + * + * Use RC_MAP_FAILSAFE to specify which channel is used to check. + * Note: The default value of 0 is below the epxed range and hence disables the feature. + * + * @min 0 + * @max 2200 + * @unit us + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FAILS_THR, 0); + +/** + * Threshold for selecting assist mode + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channel 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 1000); + +/** + * Max input value for RSSI reading. + * + * Only used if RC_RSSI_PWM_CHAN > 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 2000); diff --git a/src/prometheus_px4/src/modules/rc_update/params_deprecated.c b/src/prometheus_px4/src/modules/rc_update/params_deprecated.c new file mode 100644 index 0000000..7664aee --- /dev/null +++ b/src/prometheus_px4/src/modules/rc_update/params_deprecated.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Rattitude switch channel (deprecated) + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0); diff --git a/src/prometheus_px4/src/modules/rc_update/rc_update.cpp b/src/prometheus_px4/src/modules/rc_update/rc_update.cpp new file mode 100644 index 0000000..7e80417 --- /dev/null +++ b/src/prometheus_px4/src/modules/rc_update/rc_update.cpp @@ -0,0 +1,742 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 rc_update.cpp + * + * @author Beat Kueng + */ + +#include "rc_update.h" + +using namespace time_literals; + +namespace RCUpdate +{ + +// TODO: find a better home for this +static bool operator ==(const manual_control_switches_s &a, const manual_control_switches_s &b) +{ + return (a.mode_slot == b.mode_slot && + a.mode_switch == b.mode_switch && + a.return_switch == b.return_switch && + a.posctl_switch == b.posctl_switch && + a.loiter_switch == b.loiter_switch && + a.acro_switch == b.acro_switch && + a.offboard_switch == b.offboard_switch && + a.kill_switch == b.kill_switch && + a.arm_switch == b.arm_switch && + a.transition_switch == b.transition_switch && + a.gear_switch == b.gear_switch && + a.stab_switch == b.stab_switch && + a.man_switch == b.man_switch); +} + +static bool operator !=(const manual_control_switches_s &a, const manual_control_switches_s &b) { return !(a == b); } + + +RCUpdate::RCUpdate() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ + // initialize parameter handles + for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) { + char nbuf[16]; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles.min[i] = param_find(nbuf); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles.trim[i] = param_find(nbuf); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles.max[i] = param_find(nbuf); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles.rev[i] = param_find(nbuf); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles.dz[i] = param_find(nbuf); + } + + // RC to parameter mapping for changing parameters with RC + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + // shifted by 1 because param name starts at 1 + char name[rc_parameter_map_s::PARAM_ID_LEN]; + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); + _parameter_handles.rc_map_param[i] = param_find(name); + } + + rc_parameter_map_poll(true /* forced */); + + parameters_updated(); +} + +RCUpdate::~RCUpdate() +{ + perf_free(_loop_perf); + perf_free(_loop_interval_perf); + perf_free(_valid_data_interval_perf); +} + +bool RCUpdate::init() +{ + if (!_input_rc_sub.registerCallback()) { + PX4_ERR("input_rc callback registration failed!"); + return false; + } + + return true; +} + +void RCUpdate::parameters_updated() +{ + // rc values + for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) { + + float min = 0.f; + param_get(_parameter_handles.min[i], &min); + _parameters.min[i] = min; + + float trim = 0.f; + param_get(_parameter_handles.trim[i], &trim); + _parameters.trim[i] = trim; + + float max = 0.f; + param_get(_parameter_handles.max[i], &max); + _parameters.max[i] = max; + + float rev = 0.f; + param_get(_parameter_handles.rev[i], &rev); + _parameters.rev[i] = (rev < 0.f); + + float dz = 0.f; + param_get(_parameter_handles.dz[i], &dz); + _parameters.dz[i] = dz; + } + + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); + } + + update_rc_functions(); + + // deprecated parameters, will be removed post v1.12 once QGC is updated + { + int32_t rc_map_value = 0; + + if (param_get(param_find("RC_MAP_RATT_SW"), &rc_map_value) == PX4_OK) { + if (rc_map_value != 0) { + PX4_WARN("RC_MAP_RATT_SW deprecated"); + param_reset(param_find("RC_MAP_RATT_SW")); + } + } + } +} + +void RCUpdate::update_rc_functions() +{ + /* update RC function mappings */ + _rc.function[rc_channels_s::FUNCTION_THROTTLE] = _param_rc_map_throttle.get() - 1; + _rc.function[rc_channels_s::FUNCTION_ROLL] = _param_rc_map_roll.get() - 1; + _rc.function[rc_channels_s::FUNCTION_PITCH] = _param_rc_map_pitch.get() - 1; + _rc.function[rc_channels_s::FUNCTION_YAW] = _param_rc_map_yaw.get() - 1; + + _rc.function[rc_channels_s::FUNCTION_MODE] = _param_rc_map_mode_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_RETURN] = _param_rc_map_return_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_POSCTL] = _param_rc_map_posctl_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_LOITER] = _param_rc_map_loiter_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_ACRO] = _param_rc_map_acro_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_OFFBOARD] = _param_rc_map_offb_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_KILLSWITCH] = _param_rc_map_kill_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_ARMSWITCH] = _param_rc_map_arm_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_TRANSITION] = _param_rc_map_trans_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_GEAR] = _param_rc_map_gear_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_STAB] = _param_rc_map_stab_sw.get() - 1; + _rc.function[rc_channels_s::FUNCTION_MAN] = _param_rc_map_man_sw.get() - 1; + + _rc.function[rc_channels_s::FUNCTION_FLAPS] = _param_rc_map_flaps.get() - 1; + + _rc.function[rc_channels_s::FUNCTION_AUX_1] = _param_rc_map_aux1.get() - 1; + _rc.function[rc_channels_s::FUNCTION_AUX_2] = _param_rc_map_aux2.get() - 1; + _rc.function[rc_channels_s::FUNCTION_AUX_3] = _param_rc_map_aux3.get() - 1; + _rc.function[rc_channels_s::FUNCTION_AUX_4] = _param_rc_map_aux4.get() - 1; + _rc.function[rc_channels_s::FUNCTION_AUX_5] = _param_rc_map_aux5.get() - 1; + _rc.function[rc_channels_s::FUNCTION_AUX_6] = _param_rc_map_aux6.get() - 1; + + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + _rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + } +} + +void RCUpdate::rc_parameter_map_poll(bool forced) +{ + if (_rc_parameter_map_sub.updated() || forced) { + _rc_parameter_map_sub.copy(&_rc_parameter_map); + + /* update parameter handles to which the RC channels are mapped */ + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + /* Set the handle by index if the index is set, otherwise use the id */ + if (_rc_parameter_map.param_index[i] >= 0) { + _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]); + + } else { + _parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]); + } + } + + PX4_DEBUG("rc to parameter map updated"); + + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + PX4_DEBUG("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", + i, + &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] + ); + } + } +} + +float RCUpdate::get_rc_value(uint8_t func, float min_value, float max_value) const +{ + if (_rc.function[func] >= 0) { + return math::constrain(_rc.channels[_rc.function[func]], min_value, max_value); + } + + return 0.f; +} + +void RCUpdate::set_params_from_rc() +{ + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + float rc_val = get_rc_value((rc_channels_s::FUNCTION_PARAM_1 + i), -1.f, 1.f); + + /* Check if the value has changed, + * maybe we need to introduce a more aggressive limit here */ + if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) { + _param_rc_values[i] = rc_val; + float param_val = math::constrain( + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); + + param_set(_parameter_handles.rc_param[i], ¶m_val); + } + } +} + +void RCUpdate::Run() +{ + if (should_exit()) { + _input_rc_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); + + // 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(); + parameters_updated(); + } + + rc_parameter_map_poll(); + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + input_rc_s input_rc; + + if (_input_rc_sub.update(&input_rc)) { + + // warn if the channel count is changing (possibly indication of error) + if (!input_rc.rc_lost) { + if ((_channel_count_previous != input_rc.channel_count) + && (_channel_count_previous > 0)) { + PX4_WARN("channel count changed %d -> %d", _channel_count_previous, input_rc.channel_count); + } + + if ((_input_source_previous != input_rc.input_source) + && (_input_source_previous != input_rc_s::RC_INPUT_SOURCE_UNKNOWN)) { + PX4_WARN("input source changed %d -> %d", _input_source_previous, input_rc.input_source); + } + } + + const bool input_source_stable = (input_rc.input_source == _input_source_previous); + const bool channel_count_stable = (input_rc.channel_count == _channel_count_previous); + + _input_source_previous = input_rc.input_source; + _channel_count_previous = input_rc.channel_count; + + const uint8_t channel_count_limited = math::min(input_rc.channel_count, RC_MAX_CHAN_COUNT); + + if (channel_count_limited > _channel_count_max) { + _channel_count_max = channel_count_limited; + } + + /* detect RC signal loss */ + bool signal_lost = true; + + /* check flags and require at least four channels to consider the signal valid */ + if (input_rc.rc_lost || input_rc.rc_failsafe || input_rc.channel_count < 4) { + /* signal is lost or no enough channels */ + signal_lost = true; + + } else if ((input_rc.input_source == input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM || + input_rc.input_source == input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM) + && input_rc.channel_count == 16) { + + // This is a specific RC lost check for RFD 868+/900 Modems on PPM. + // The observation was that when RC is lost, 16 channels are active and the first 12 are 1000 + // and the remaining ones are 0. + for (unsigned int i = 0; i < 16; i++) { + if (i < 12 && input_rc.values[i] > 999 && input_rc.values[i] < 1005) { + signal_lost = true; + + } else if (input_rc.values[i] == 0) { + signal_lost = true; + + } else { + signal_lost = false; + break; + } + } + + } else { + /* signal looks good */ + signal_lost = false; + + /* check failsafe */ + int8_t fs_ch = _rc.function[_param_rc_map_failsafe.get()]; // get channel mapped to throttle + + if (_param_rc_map_failsafe.get() > 0) { // if not 0, use channel number instead of rc.function mapping + fs_ch = _param_rc_map_failsafe.get() - 1; + } + + if (_param_rc_fails_thr.get() > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_param_rc_fails_thr.get() < _parameters.min[fs_ch] && input_rc.values[fs_ch] < _param_rc_fails_thr.get()) || + (_param_rc_fails_thr.get() > _parameters.max[fs_ch] && input_rc.values[fs_ch] > _param_rc_fails_thr.get())) { + /* failsafe triggered, signal is lost by receiver */ + signal_lost = true; + } + } + } + + /* read out and scale values from raw message even if signal is invalid */ + for (unsigned int i = 0; i < channel_count_limited; i++) { + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + input_rc.values[i] = math::constrain(input_rc.values[i], _parameters.min[i], _parameters.max[i]); + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * the total range is 2 (-1..1). + * If center (trim) == min, scale to 0..1, if center (trim) == max, + * scale to -1..0. + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (input_rc.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { + _rc.channels[i] = (input_rc.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)( + _parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); + + } else if (input_rc.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { + _rc.channels[i] = (input_rc.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)( + _parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); + + } else { + /* in the configured dead zone, output zero */ + _rc.channels[i] = 0.f; + } + + if (_parameters.rev[i]) { + _rc.channels[i] = -_rc.channels[i]; + } + + + /* handle any parameter-induced blowups */ + if (!PX4_ISFINITE(_rc.channels[i])) { + _rc.channels[i] = 0.f; + } + } + + _rc.channel_count = input_rc.channel_count; + _rc.rssi = input_rc.rssi; + _rc.signal_lost = signal_lost; + _rc.timestamp = input_rc.timestamp_last_signal; + _rc.frame_drop_count = input_rc.rc_lost_frame_count; + + /* publish rc_channels topic even if signal is invalid, for debug */ + _rc_channels_pub.publish(_rc); + + // only publish manual control if the signal is present and regularly updating + if (input_source_stable && channel_count_stable && !signal_lost) { + + if ((input_rc.timestamp_last_signal > _last_timestamp_signal) + && (input_rc.timestamp_last_signal - _last_timestamp_signal < 1_s)) { + + perf_count(_valid_data_interval_perf); + + // check if channels actually updated + bool rc_updated = false; + + for (unsigned i = 0; i < channel_count_limited; i++) { + if (_rc_values_previous[i] != input_rc.values[i]) { + rc_updated = true; + break; + } + } + + // limit processing if there's no update + if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) { + UpdateManualSetpoint(input_rc.timestamp_last_signal); + } + + UpdateManualSwitches(input_rc.timestamp_last_signal); + + /* Update parameters from RC Channels (tuning with RC) if activated */ + if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) { + set_params_from_rc(); + _last_rc_to_param_map_time = hrt_absolute_time(); + } + } + + _last_timestamp_signal = input_rc.timestamp_last_signal; + } + + memcpy(_rc_values_previous, input_rc.values, sizeof(input_rc.values[0]) * channel_count_limited); + static_assert(sizeof(_rc_values_previous) == sizeof(input_rc.values), "check sizeof(_rc_values_previous)"); + } + + perf_end(_loop_perf); +} + +switch_pos_t RCUpdate::get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const +{ + if (_rc.function[func] >= 0) { + const bool on_inv = (on_th < 0.f); + const bool mid_inv = (mid_th < 0.f); + + const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return manual_control_switches_s::SWITCH_POS_ON; + + } else if (mid_inv ? value < mid_th : value > mid_th) { + return manual_control_switches_s::SWITCH_POS_MIDDLE; + + } else { + return manual_control_switches_s::SWITCH_POS_OFF; + } + } + + return manual_control_switches_s::SWITCH_POS_NONE; +} + +switch_pos_t RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th) const +{ + if (_rc.function[func] >= 0) { + const bool on_inv = (on_th < 0.f); + + const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return manual_control_switches_s::SWITCH_POS_ON; + + } else { + return manual_control_switches_s::SWITCH_POS_OFF; + } + } + + return manual_control_switches_s::SWITCH_POS_NONE; +} + +void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) +{ + manual_control_switches_s switches{}; + switches.timestamp_sample = timestamp_sample; + + // check mode slot (RC_MAP_FLTMODE) or legacy mode switch (RC_MAP_MODE_SW), but not both + if (_param_rc_map_fltmode.get() > 0) { + // number of valid slots + static constexpr int num_slots = manual_control_switches_s::MODE_SLOT_NUM; + + // the half width of the range of a slot is the total range + // divided by the number of slots, again divided by two + static constexpr float slot_width_half = 1.f / num_slots; + + // min is -1, max is +1, range is 2. We offset below min and max + static constexpr float slot_min = -1.f - 0.05f; + static constexpr float slot_max = 1.f + 0.05f; + + // the slot gets mapped by first normalizing into a 0..1 interval using min + // and max. Then the right slot is obtained by multiplying with the number of + // slots. And finally we add half a slot width to ensure that integer rounding + // will take us to the correct final index. + const float value = _rc.channels[_param_rc_map_fltmode.get() - 1]; + switches.mode_slot = (((((value - slot_min) * num_slots) + slot_width_half) / (slot_max - slot_min)) + + slot_width_half) + 1; + + if (switches.mode_slot > num_slots) { + switches.mode_slot = num_slots; + } + + } else if (_param_rc_map_mode_sw.get() > 0) { + switches.mode_switch = get_rc_sw3pos_position(rc_channels_s::FUNCTION_MODE, + _param_rc_auto_th.get(), _param_rc_assist_th.get()); + + // only used with legacy mode switch + switches.man_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_MAN, _param_rc_man_th.get()); + switches.acro_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ACRO, _param_rc_acro_th.get()); + switches.stab_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_STAB, _param_rc_stab_th.get()); + switches.posctl_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_POSCTL, _param_rc_posctl_th.get()); + } + + switches.return_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get()); + switches.loiter_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_LOITER, _param_rc_loiter_th.get()); + switches.offboard_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_OFFBOARD, _param_rc_offb_th.get()); + switches.kill_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_KILLSWITCH, _param_rc_killswitch_th.get()); + switches.arm_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ARMSWITCH, _param_rc_armswitch_th.get()); + switches.transition_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get()); + switches.gear_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_GEAR, _param_rc_gear_th.get()); + + // last 2 switch updates identical (simple protection from bad RC data) + if (switches == _manual_switches_previous) { + const bool switches_changed = (switches != _manual_switches_last_publish); + + // publish immediately on change or at ~1 Hz + if (switches_changed || (hrt_elapsed_time(&_manual_switches_last_publish.timestamp) >= 1_s)) { + uint32_t switch_changes = _manual_switches_last_publish.switch_changes; + + if (switches_changed) { + switch_changes++; + } + + _manual_switches_last_publish = switches; + _manual_switches_last_publish.switch_changes = switch_changes; + _manual_switches_last_publish.timestamp_sample = _manual_switches_previous.timestamp_sample; + _manual_switches_last_publish.timestamp = hrt_absolute_time(); + _manual_control_switches_pub.publish(_manual_switches_last_publish); + } + } + + _manual_switches_previous = switches; +} + +void RCUpdate::UpdateManualSetpoint(const hrt_abstime ×tamp_sample) +{ + manual_control_setpoint_s manual_control_setpoint{}; + manual_control_setpoint.timestamp_sample = timestamp_sample; + manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC; + + // limit controls + manual_control_setpoint.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); + manual_control_setpoint.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f); + manual_control_setpoint.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f); + manual_control_setpoint.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f); + manual_control_setpoint.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f); + manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f); + manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f); + manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::FUNCTION_AUX_3, -1.f, 1.f); + manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::FUNCTION_AUX_4, -1.f, 1.f); + manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::FUNCTION_AUX_5, -1.f, 1.f); + manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::FUNCTION_AUX_6, -1.f, 1.f); + + // publish manual_control_setpoint topic + manual_control_setpoint.timestamp = hrt_absolute_time(); + _manual_control_setpoint_pub.publish(manual_control_setpoint); + _last_manual_control_setpoint_publish = manual_control_setpoint.timestamp; + + + actuator_controls_s actuator_group_3{}; + // copy in previous actuator control setpoint in case aux{1, 2, 3} isn't changed + _actuator_controls_3_sub.update(&actuator_group_3); + // populate and publish actuator_controls_3 copied from mapped manual_control_setpoint + actuator_group_3.control[0] = manual_control_setpoint.y; + actuator_group_3.control[1] = manual_control_setpoint.x; + actuator_group_3.control[2] = manual_control_setpoint.r; + actuator_group_3.control[3] = manual_control_setpoint.z; + actuator_group_3.control[4] = manual_control_setpoint.flaps; + + float new_aux_values[3]; + new_aux_values[0] = manual_control_setpoint.aux1; + new_aux_values[1] = manual_control_setpoint.aux2; + new_aux_values[2] = manual_control_setpoint.aux3; + + // if AUX RC was already active, we update. otherwise, we check + // if there is a major stick movement to re-activate RC mode + bool major_movement[3] = {false, false, false}; + + // detect a big stick movement + for (int i = 0; i < 3; i++) { + if (fabsf(_last_manual_control_setpoint[i] - new_aux_values[i]) > 0.1f) { + major_movement[i] = true; + } + } + + for (int i = 0; i < 3; i++) { + // if someone else (DO_SET_ACTUATOR) updated the actuator control + // and we haven't had a major movement, switch back to automatic control + if ((fabsf(_last_manual_control_setpoint[i] - actuator_group_3.control[5 + i]) + > 0.0001f) && (!major_movement[i])) { + _aux_already_active[i] = false; + } + + if (_aux_already_active[i] || major_movement[i]) { + _aux_already_active[i] = true; + _last_manual_control_setpoint[i] = new_aux_values[i]; + + actuator_group_3.control[5 + i] = new_aux_values[i]; + } + } + + actuator_group_3.timestamp = hrt_absolute_time(); + _actuator_group_3_pub.publish(actuator_group_3); +} + +int RCUpdate::task_spawn(int argc, char *argv[]) +{ + RCUpdate *instance = new RCUpdate(); + + 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 RCUpdate::print_status() +{ + PX4_INFO_RAW("Running\n"); + + if (_channel_count_max > 0) { + PX4_INFO_RAW(" # MIN MAX TRIM DZ REV\n"); + + for (int i = 0; i < _channel_count_max; i++) { + PX4_INFO_RAW("%2d %4d %4d %4d %3d %3d\n", i, _parameters.min[i], _parameters.max[i], _parameters.trim[i], + _parameters.dz[i], _parameters.rev[i]); + } + } + + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + perf_print_counter(_valid_data_interval_perf); + + return 0; +} + +int RCUpdate::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int RCUpdate::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The rc_update module handles RC channel mapping: read the raw input channels (`input_rc`), +then apply the calibration, map the RC channels to the configured channels & mode switches +and then publish as `rc_channels` and `manual_control_setpoint`. + +### Implementation +To reduce control latency, the module is scheduled on input_rc publications. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rc_update", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +} // namespace RCUpdate + +extern "C" __EXPORT int rc_update_main(int argc, char *argv[]) +{ + return RCUpdate::RCUpdate::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/rc_update/rc_update.h b/src/prometheus_px4/src/modules/rc_update/rc_update.h new file mode 100644 index 0000000..93324cb --- /dev/null +++ b/src/prometheus_px4/src/modules/rc_update/rc_update.h @@ -0,0 +1,246 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 + +/** + * @file rc_update.h + * + * @author Beat Kueng + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace RCUpdate +{ + +/** + ** class RCUpdate + * + * Handling of RC updates + */ +class RCUpdate : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + RCUpdate(); + ~RCUpdate() 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(); + + int print_status() override; + +private: + + void Run() override; + + /** + * Check for changes in rc_parameter_map + */ + void rc_parameter_map_poll(bool forced = false); + + /** + * update the RC functions. Call this when the parameters change. + */ + void update_rc_functions(); + + void UpdateManualSetpoint(const hrt_abstime ×tamp_sample); + void UpdateManualSwitches(const hrt_abstime ×tamp_sample); + + /** + * Update our local parameter cache. + */ + void parameters_updated(); + + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(uint8_t func, float min_value, float max_value) const; + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const; + switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th) const; + + /** + * Update parameters from RC channels if the functionality is activated and the + * input has changed since the last update + * + * @param + */ + void set_params_from_rc(); + + static constexpr uint8_t RC_MAX_CHAN_COUNT{input_rc_s::RC_INPUT_MAX_CHANNELS}; /**< maximum number of r/c channels we handle */ + + struct Parameters { + uint16_t min[RC_MAX_CHAN_COUNT]; + uint16_t trim[RC_MAX_CHAN_COUNT]; + uint16_t max[RC_MAX_CHAN_COUNT]; + uint16_t dz[RC_MAX_CHAN_COUNT]; + bool rev[RC_MAX_CHAN_COUNT]; + + int32_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + } _parameters{}; + + struct ParameterHandles { + param_t min[RC_MAX_CHAN_COUNT]; + param_t trim[RC_MAX_CHAN_COUNT]; + param_t max[RC_MAX_CHAN_COUNT]; + param_t rev[RC_MAX_CHAN_COUNT]; + param_t dz[RC_MAX_CHAN_COUNT]; + + param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the parameters which are bound + to a RC channel, equivalent float values in the + _parameters struct are not existing + because these parameters are never read. */ + } _parameter_handles{}; + + uORB::SubscriptionCallbackWorkItem _input_rc_sub{this, ORB_ID(input_rc)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)}; + uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)}; + + uORB::Publication _rc_channels_pub{ORB_ID(rc_channels)}; + uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + uORB::Publication _manual_control_switches_pub{ORB_ID(manual_control_switches)}; + uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; + + manual_control_switches_s _manual_switches_previous{}; + manual_control_switches_s _manual_switches_last_publish{}; + rc_channels_s _rc{}; + + rc_parameter_map_s _rc_parameter_map {}; + float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {}; /**< parameter values for RC control */ + + hrt_abstime _last_manual_control_setpoint_publish{0}; + hrt_abstime _last_rc_to_param_map_time{0}; + hrt_abstime _last_timestamp_signal{0}; + + uint16_t _rc_values_previous[RC_MAX_CHAN_COUNT] {}; + float _last_manual_control_setpoint[3] {}; + bool _aux_already_active[3] = {false, false, false}; + + uint8_t _channel_count_previous{0}; + uint8_t _input_source_previous{input_rc_s::RC_INPUT_SOURCE_UNKNOWN}; + + uint8_t _channel_count_max{0}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + perf_counter_t _valid_data_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": valid data interval")}; + + DEFINE_PARAMETERS( + + (ParamInt) _param_rc_map_roll, + (ParamInt) _param_rc_map_pitch, + (ParamInt) _param_rc_map_yaw, + (ParamInt) _param_rc_map_throttle, + (ParamInt) _param_rc_map_failsafe, + + (ParamInt) _param_rc_map_fltmode, + (ParamInt) _param_rc_map_mode_sw, + + (ParamInt) _param_rc_map_flaps, + + (ParamInt) _param_rc_map_return_sw, + (ParamInt) _param_rc_map_posctl_sw, + (ParamInt) _param_rc_map_loiter_sw, + (ParamInt) _param_rc_map_acro_sw, + (ParamInt) _param_rc_map_offb_sw, + (ParamInt) _param_rc_map_kill_sw, + (ParamInt) _param_rc_map_arm_sw, + (ParamInt) _param_rc_map_trans_sw, + (ParamInt) _param_rc_map_gear_sw, + (ParamInt) _param_rc_map_stab_sw, + (ParamInt) _param_rc_map_man_sw, + + (ParamInt) _param_rc_map_aux1, + (ParamInt) _param_rc_map_aux2, + (ParamInt) _param_rc_map_aux3, + (ParamInt) _param_rc_map_aux4, + (ParamInt) _param_rc_map_aux5, + (ParamInt) _param_rc_map_aux6, + + (ParamInt) _param_rc_fails_thr, + + (ParamFloat) _param_rc_assist_th, + (ParamFloat) _param_rc_auto_th, + (ParamFloat) _param_rc_posctl_th, + (ParamFloat) _param_rc_loiter_th, + (ParamFloat) _param_rc_acro_th, + (ParamFloat) _param_rc_offb_th, + (ParamFloat) _param_rc_killswitch_th, + (ParamFloat) _param_rc_armswitch_th, + (ParamFloat) _param_rc_trans_th, + (ParamFloat) _param_rc_gear_th, + (ParamFloat) _param_rc_stab_th, + (ParamFloat) _param_rc_man_th, + (ParamFloat) _param_rc_return_th, + + (ParamInt) _param_rc_chan_cnt + ) +}; +} /* namespace RCUpdate */ diff --git a/src/prometheus_px4/src/modules/replay/CMakeLists.txt b/src/prometheus_px4/src/modules/replay/CMakeLists.txt new file mode 100644 index 0000000..1e83c88 --- /dev/null +++ b/src/prometheus_px4/src/modules/replay/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2016-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__replay + MAIN replay + COMPILE_FLAGS + SRCS + definitions.hpp + replay_main.cpp + Replay.cpp + Replay.hpp + ReplayEkf2.cpp + ReplayEkf2.hpp + ) diff --git a/src/prometheus_px4/src/modules/replay/Replay.cpp b/src/prometheus_px4/src/modules/replay/Replay.cpp new file mode 100644 index 0000000..8869b0a --- /dev/null +++ b/src/prometheus_px4/src/modules/replay/Replay.cpp @@ -0,0 +1,1115 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 Replay.cpp + * This module reads messages from an ULog file and publishes them. + * It sets the parameters from the log file and handles user-defined + * parameter overrides. + * + * @author Beat Kueng +*/ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "Replay.hpp" +#include "ReplayEkf2.hpp" + +#define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt" + +using namespace std; +using namespace time_literals; + +namespace px4 +{ + +char *Replay::_replay_file = nullptr; + +Replay::CompatSensorCombinedDtType::CompatSensorCombinedDtType(int gyro_integral_dt_offset_log, + int gyro_integral_dt_offset_intern, int accelerometer_integral_dt_offset_log, + int accelerometer_integral_dt_offset_intern) + : _gyro_integral_dt_offset_log(gyro_integral_dt_offset_log), + _gyro_integral_dt_offset_intern(gyro_integral_dt_offset_intern), + _accelerometer_integral_dt_offset_log(accelerometer_integral_dt_offset_log), + _accelerometer_integral_dt_offset_intern(accelerometer_integral_dt_offset_intern) +{ +} + +Replay::~Replay() +{ + for (size_t i = 0; i < _subscriptions.size(); ++i) { + delete (_subscriptions[i]); + } + + _subscriptions.clear(); +} + +void * +Replay::CompatSensorCombinedDtType::apply(void *data) +{ + // the types have the same size so we can do the conversion in-place + uint8_t *ptr = (uint8_t *)data; + + float gyro_integral_dt; + memcpy(&gyro_integral_dt, ptr + _gyro_integral_dt_offset_log, sizeof(float)); + + float accel_integral_dt; + memcpy(&accel_integral_dt, ptr + _accelerometer_integral_dt_offset_log, sizeof(float)); + + uint32_t igyro_integral_dt = (uint32_t)(gyro_integral_dt * 1e6f); + memcpy(ptr + _gyro_integral_dt_offset_intern, &igyro_integral_dt, sizeof(float)); + + uint32_t iaccel_integral_dt = (uint32_t)(accel_integral_dt * 1e6f); + memcpy(ptr + _accelerometer_integral_dt_offset_intern, &iaccel_integral_dt, sizeof(float)); + + return data; +} + +void +Replay::setupReplayFile(const char *file_name) +{ + if (_replay_file) { + free(_replay_file); + } + + _replay_file = strdup(file_name); +} + +void +Replay::setUserParams(const char *filename) +{ + string line; + string pname; + string value_string; + ifstream myfile(filename); + + if (!myfile.is_open()) { + return; + } + + PX4_INFO("Applying override params from %s...", filename); + + while (!myfile.eof()) { + getline(myfile, line); + + if (line.empty() || line[0] == '#') { + continue; + } + + istringstream mystrstream(line); + mystrstream >> pname; + mystrstream >> value_string; + + double param_value_double = stod(value_string); + + param_t handle = param_find(pname.c_str()); + param_type_t param_format = param_type(handle); + _overridden_params.insert(pname); + + if (param_format == PARAM_TYPE_INT32) { + int32_t orig_value = 0; + param_get(handle, &orig_value); + + int32_t value = (int32_t)param_value_double; + + if (orig_value != value) { + PX4_WARN("setting %s (INT32) %d -> %d", param_name(handle), orig_value, value); + } + + param_set(handle, (const void *)&value); + + } else if (param_format == PARAM_TYPE_FLOAT) { + float orig_value = 0; + param_get(handle, &orig_value); + + float value = (float)param_value_double; + + if (fabsf(orig_value - value) > FLT_EPSILON) { + PX4_WARN("setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value); + } + + param_set(handle, (const void *)&value); + } + } +} + +bool +Replay::readFileHeader(std::ifstream &file) +{ + file.seekg(0); + ulog_file_header_s msg_header; + file.read((char *)&msg_header, sizeof(msg_header)); + + if (!file) { + return false; + } + + _file_start_time = msg_header.timestamp; + //verify it's an ULog file + char magic[8]; + magic[0] = 'U'; + magic[1] = 'L'; + magic[2] = 'o'; + magic[3] = 'g'; + magic[4] = 0x01; + magic[5] = 0x12; + magic[6] = 0x35; + return memcmp(magic, msg_header.magic, 7) == 0; +} + +bool +Replay::readFileDefinitions(std::ifstream &file) +{ + PX4_INFO("Applying params from ULog file..."); + + ulog_message_header_s message_header; + file.seekg(sizeof(ulog_file_header_s)); + + while (true) { + file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (!file) { + return false; + } + + switch (message_header.msg_type) { + case (int)ULogMessageType::FLAG_BITS: + if (!readFlagBits(file, message_header.msg_size)) { + return false; + } + + break; + + case (int)ULogMessageType::FORMAT: + if (!readFormat(file, message_header.msg_size)) { + return false; + } + + break; + + case (int)ULogMessageType::PARAMETER: + if (!readAndApplyParameter(file, message_header.msg_size)) { + return false; + } + + break; + + case (int)ULogMessageType::ADD_LOGGED_MSG: + _data_section_start = file.tellg() - (streamoff)ULOG_MSG_HEADER_LEN; + return true; + + case (int)ULogMessageType::INFO: //skip + case (int)ULogMessageType::INFO_MULTIPLE: //skip + file.seekg(message_header.msg_size, ios::cur); + break; + + default: + PX4_ERR("unknown log definition type %i, size %i (offset %i)", + (int)message_header.msg_type, (int)message_header.msg_size, (int)file.tellg()); + file.seekg(message_header.msg_size, ios::cur); + break; + } + } + + return true; +} + +bool +Replay::readFlagBits(std::ifstream &file, uint16_t msg_size) +{ + if (msg_size != 40) { + PX4_ERR("unsupported message length for FLAG_BITS message (%i)", msg_size); + return false; + } + + _read_buffer.reserve(msg_size); + uint8_t *message = (uint8_t *)_read_buffer.data(); + file.read((char *)message, msg_size); + //uint8_t *compat_flags = message; + uint8_t *incompat_flags = message + 8; + + // handle & validate the flags + bool contains_appended_data = incompat_flags[0] & ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK; + bool has_unknown_incompat_bits = false; + + if (incompat_flags[0] & ~0x1) { + has_unknown_incompat_bits = true; + } + + for (int i = 1; i < 8; ++i) { + if (incompat_flags[i]) { + has_unknown_incompat_bits = true; + } + } + + if (has_unknown_incompat_bits) { + PX4_ERR("Log contains unknown incompat bits set. Refusing to parse"); + return false; + } + + if (contains_appended_data) { + uint64_t appended_offsets[3]; + memcpy(appended_offsets, message + 16, sizeof(appended_offsets)); + + if (appended_offsets[0] > 0) { + // the appended data is currently only used for hardfault dumps, so it's safe to ignore it. + PX4_INFO("Log contains appended data. Replay will ignore this data"); + _read_until_file_position = appended_offsets[0]; + } + } + + return true; +} + +bool +Replay::readFormat(std::ifstream &file, uint16_t msg_size) +{ + _read_buffer.reserve(msg_size + 1); + char *format = (char *)_read_buffer.data(); + file.read(format, msg_size); + format[msg_size] = 0; + + if (!file) { + return false; + } + + string str_format(format); + size_t pos = str_format.find(':'); + + if (pos == string::npos) { + return false; + } + + string name = str_format.substr(0, pos); + string fields = str_format.substr(pos + 1); + _file_formats[name] = fields; + + return true; +} + +bool +Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) +{ + _read_buffer.reserve(msg_size + 1); + char *message = (char *)_read_buffer.data(); + streampos this_message_pos = file.tellg() - (streamoff)ULOG_MSG_HEADER_LEN; + file.read(message, msg_size); + message[msg_size] = 0; + + if (!file) { + return false; + } + + if (file.tellg() <= _subscription_file_pos) { //already read this subscription + return true; + } + + _subscription_file_pos = file.tellg(); + + uint8_t multi_id = *(uint8_t *)message; + uint16_t msg_id = ((uint16_t) message[1]) | (((uint16_t) message[2]) << 8); + string topic_name(message + 3); + const orb_metadata *orb_meta = findTopic(topic_name); + + if (!orb_meta) { + PX4_WARN("Topic %s not found internally. Will ignore it", topic_name.c_str()); + return true; + } + + CompatBase *compat = nullptr; + + // check the format: the field definitions must match + // FIXME: this should check recursively, all used nested types + string file_format = _file_formats[topic_name]; + + if (file_format != orb_meta->o_fields) { + // check if we have a compatibility conversion available + if (topic_name == "sensor_combined") { + if (string(orb_meta->o_fields) == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;" + "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;" + "uint32_t accelerometer_integral_dt" && + file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;" + "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;" + "float accelerometer_integral_dt;") { + + int gyro_integral_dt_offset_log; + int gyro_integral_dt_offset_intern; + int accelerometer_integral_dt_offset_log; + int accelerometer_integral_dt_offset_intern; + int unused; + + if (findFieldOffset(file_format, "gyro_integral_dt", gyro_integral_dt_offset_log, unused) && + findFieldOffset(orb_meta->o_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) && + findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) && + findFieldOffset(orb_meta->o_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) { + + compat = new CompatSensorCombinedDtType(gyro_integral_dt_offset_log, gyro_integral_dt_offset_intern, + accelerometer_integral_dt_offset_log, accelerometer_integral_dt_offset_intern); + } + } + } + + if (!compat) { + PX4_ERR("Formats for %s don't match. Will ignore it.", topic_name.c_str()); + PX4_WARN(" Internal format: %s", orb_meta->o_fields); + PX4_WARN(" File format : %s", file_format.c_str()); + return true; // not a fatal error + } + } + + Subscription *subscription = new Subscription(); + subscription->orb_meta = orb_meta; + subscription->multi_id = multi_id; + subscription->compat = compat; + + //find the timestamp offset + int field_size; + bool timestamp_found = findFieldOffset(orb_meta->o_fields, "timestamp", subscription->timestamp_offset, field_size); + + if (!timestamp_found) { + return true; + } + + if (field_size != 8) { + PX4_ERR("Unsupported timestamp with size %i, ignoring the topic %s", field_size, orb_meta->o_name); + return true; + } + + //find first data message (and the timestamp) + streampos cur_pos = file.tellg(); + subscription->next_read_pos = this_message_pos; //this will be skipped + + if (!nextDataMessage(file, *subscription, msg_id)) { + return false; + } + + file.seekg(cur_pos); + + if (!subscription->orb_meta) { + //no message found. This is not a fatal error + return true; + } + + PX4_DEBUG("adding subscription for %s (msg_id %i)", subscription->orb_meta->o_name, msg_id); + + //add subscription + if (_subscriptions.size() <= msg_id) { + _subscriptions.resize(msg_id + 1); + } + + _subscriptions[msg_id] = subscription; + + onSubscriptionAdded(*_subscriptions[msg_id], msg_id); + + return true; +} + +bool +Replay::findFieldOffset(const string &format, const string &field_name, int &offset, int &field_size) +{ + size_t prev_field_end = 0; + size_t field_end = format.find(';'); + offset = 0; + field_size = 0; + + while (field_end != string::npos) { + size_t space_pos = format.find(' ', prev_field_end); + + if (space_pos != string::npos) { + string type_name_full = format.substr(prev_field_end, space_pos - prev_field_end); + string cur_field_name = format.substr(space_pos + 1, field_end - space_pos - 1); + + if (cur_field_name == field_name) { + field_size = sizeOfFullType(type_name_full); + return true; + + } else { + offset += sizeOfFullType(type_name_full); + } + } + + prev_field_end = field_end + 1; + field_end = format.find(';', prev_field_end); + } + + return false; +} + +bool +Replay::readAndHandleAdditionalMessages(std::ifstream &file, std::streampos end_position) +{ + ulog_message_header_s message_header; + + while (file.tellg() < end_position) { + file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (!file) { + return false; + } + + switch (message_header.msg_type) { + case (int)ULogMessageType::PARAMETER: + if (!readAndApplyParameter(file, message_header.msg_size)) { + return false; + } + + break; + + case (int)ULogMessageType::DROPOUT: + readDropout(file, message_header.msg_size); + break; + + default: //skip all others + file.seekg(message_header.msg_size, ios::cur); + break; + } + } + + return true; +} + +bool +Replay::readAndApplyParameter(std::ifstream &file, uint16_t msg_size) +{ + _read_buffer.reserve(msg_size); + uint8_t *message = (uint8_t *)_read_buffer.data(); + file.read((char *)message, msg_size); + + if (!file) { + return false; + } + + uint8_t key_len = message[0]; + string key((char *)message + 1, key_len); + + size_t pos = key.find(' '); + + if (pos == string::npos) { + return false; + } + + string type = key.substr(0, pos); + string param_name = key.substr(pos + 1); + + if (_overridden_params.find(param_name) != _overridden_params.end()) { + //this parameter is overridden, so don't apply it + return true; + } + + if (type != "int32_t" && type != "float") { + PX4_WARN("unknown parameter type %s, name %s (ignoring it)", type.c_str(), param_name.c_str()); + return true; + } + + param_t handle = param_find(param_name.c_str()); + + if (handle != PARAM_INVALID) { + param_set(handle, (const void *)(message + 1 + key_len)); + } + + return true; +} + +bool +Replay::readDropout(std::ifstream &file, uint16_t msg_size) +{ + uint16_t duration; + file.read((char *)&duration, sizeof(duration)); + + PX4_ERR("Dropout in replayed log, %i ms", (int)duration); + return file.good(); +} + +bool +Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id) +{ + ulog_message_header_s message_header; + file.seekg(subscription.next_read_pos); + //ignore the first message (it's data we already read) + file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (file) { + file.seekg(message_header.msg_size, ios::cur); + } + + uint16_t file_msg_id; + bool done = false; + + while (file && !done) { + streampos cur_pos = file.tellg(); + file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (!file) { + break; + } + + if (((streamoff)cur_pos) + ULOG_MSG_HEADER_LEN + message_header.msg_size > _read_until_file_position) { + file.setstate(std::ios::eofbit); + break; + } + + switch (message_header.msg_type) { + case (int)ULogMessageType::ADD_LOGGED_MSG: + readAndAddSubscription(file, message_header.msg_size); + break; + + case (int)ULogMessageType::DATA: + file.read((char *)&file_msg_id, sizeof(file_msg_id)); + + if (file) { + if (msg_id == file_msg_id) { + if (message_header.msg_size == subscription.orb_meta->o_size_no_padding + 2) { + subscription.next_read_pos = cur_pos; + file.seekg(subscription.timestamp_offset, ios::cur); + file.read((char *)&subscription.next_timestamp, sizeof(subscription.next_timestamp)); + done = true; + + } else { //sanity check failed! + PX4_ERR("data message %s has wrong size %i (expected %i). Skipping", + subscription.orb_meta->o_name, message_header.msg_size, + subscription.orb_meta->o_size_no_padding + 2); + file.seekg(message_header.msg_size - sizeof(file_msg_id), ios::cur); + } + + } else { //not the one we are looking for + file.seekg(message_header.msg_size - sizeof(file_msg_id), ios::cur); + } + } + + break; + + case (int)ULogMessageType::REMOVE_LOGGED_MSG: //skip these + case (int)ULogMessageType::PARAMETER: + case (int)ULogMessageType::DROPOUT: + case (int)ULogMessageType::INFO: + case (int)ULogMessageType::INFO_MULTIPLE: + case (int)ULogMessageType::SYNC: + case (int)ULogMessageType::LOGGING: + file.seekg(message_header.msg_size, ios::cur); + break; + + default: + //this really should not happen + PX4_ERR("unknown log message type %i, size %i (offset %i)", + (int)message_header.msg_type, (int)message_header.msg_size, (int)file.tellg()); + file.seekg(message_header.msg_size, ios::cur); + break; + } + } + + if (file.eof()) { //no more data messages for this subscription + subscription.orb_meta = nullptr; + file.clear(); + } + + return file.good(); +} + +const orb_metadata * +Replay::findTopic(const std::string &name) +{ + const orb_metadata *const *topics = orb_get_topics(); + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (name == topics[i]->o_name) { + return topics[i]; + } + } + + return nullptr; +} + +std::string +Replay::extractArraySize(const std::string &type_name_full, int &array_size) +{ + size_t start_pos = type_name_full.find('['); + size_t end_pos = type_name_full.find(']'); + + if (start_pos == string::npos || end_pos == string::npos) { + array_size = 1; + return type_name_full; + } + + array_size = atoi(type_name_full.substr(start_pos + 1, end_pos - start_pos - 1).c_str()); + return type_name_full.substr(0, start_pos); +} + +size_t +Replay::sizeOfType(const std::string &type_name) +{ + if (type_name == "int8_t" || type_name == "uint8_t") { + return 1; + + } else if (type_name == "int16_t" || type_name == "uint16_t") { + return 2; + + } else if (type_name == "int32_t" || type_name == "uint32_t") { + return 4; + + } else if (type_name == "int64_t" || type_name == "uint64_t") { + return 8; + + } else if (type_name == "float") { + return 4; + + } else if (type_name == "double") { + return 8; + + } else if (type_name == "char" || type_name == "bool") { + return 1; + } + + const orb_metadata *orb_meta = findTopic(type_name); + + if (orb_meta) { + return orb_meta->o_size; + } + + PX4_ERR("unknown type: %s", type_name.c_str()); + return 0; +} + +size_t +Replay::sizeOfFullType(const std::string &type_name_full) +{ + int array_size; + string type_name = extractArraySize(type_name_full, array_size); + return sizeOfType(type_name) * array_size; +} + +bool +Replay::readDefinitionsAndApplyParams(std::ifstream &file) +{ + // log reader currently assumes little endian + int num = 1; + + if (*(char *)&num != 1) { + PX4_ERR("Replay only works on little endian!"); + return false; + } + + if (!file.is_open()) { + PX4_ERR("Failed to open replay file"); + return false; + } + + if (!readFileHeader(file)) { + PX4_ERR("Failed to read file header. Not a valid ULog file"); + return false; + } + + //initialize the formats and apply the parameters from the log file + if (!readFileDefinitions(file)) { + PX4_ERR("Failed to read ULog definitions section. Broken file?"); + return false; + } + + setUserParams(PARAMS_OVERRIDE_FILE); + return true; +} + +void +Replay::run() +{ + ifstream replay_file(_replay_file, ios::in | ios::binary); + + if (!readDefinitionsAndApplyParams(replay_file)) { + return; + } + + _speed_factor = 1.f; + const char *speedup = getenv("PX4_SIM_SPEED_FACTOR"); + + if (speedup) { + _speed_factor = atof(speedup); + } + + onEnterMainLoop(); + + _replay_start_time = hrt_absolute_time(); + + PX4_INFO("Replay in progress..."); + + ulog_message_header_s message_header; + replay_file.seekg(_data_section_start); + + //we know the next message must be an ADD_LOGGED_MSG + replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + + if (!readAndAddSubscription(replay_file, message_header.msg_size)) { + PX4_ERR("Failed to read subscription"); + return; + } + + const uint64_t timestamp_offset = getTimestampOffset(); + uint32_t nr_published_messages = 0; + streampos last_additional_message_pos = _data_section_start; + + while (!should_exit() && replay_file) { + + //Find the next message to publish. Messages from different subscriptions don't need + //to be in chronological order, so we need to check all subscriptions + uint64_t next_file_time = 0; + int next_msg_id = -1; + bool first_time = true; + + for (size_t i = 0; i < _subscriptions.size(); ++i) { + const Subscription *subscription = _subscriptions[i]; + + if (!subscription) { + continue; + } + + if (subscription->orb_meta && !subscription->ignored) { + if (first_time || subscription->next_timestamp < next_file_time) { + first_time = false; + next_msg_id = (int)i; + next_file_time = subscription->next_timestamp; + } + } + } + + if (next_msg_id == -1) { + break; //no active subscription anymore. We're done. + } + + Subscription &sub = *_subscriptions[next_msg_id]; + + if (next_file_time == 0) { + //someone didn't set the timestamp properly. Consider the message invalid + nextDataMessage(replay_file, sub, next_msg_id); + continue; + } + + //handle additional messages between last and next published data + replay_file.seekg(last_additional_message_pos); + streampos next_additional_message_pos = sub.next_read_pos; + readAndHandleAdditionalMessages(replay_file, next_additional_message_pos); + last_additional_message_pos = next_additional_message_pos; + + const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset); + + // It's time to publish + readTopicDataToBuffer(sub, replay_file); + memcpy(_read_buffer.data() + sub.timestamp_offset, &publish_timestamp, sizeof(uint64_t)); //adjust the timestamp + + if (handleTopicUpdate(sub, _read_buffer.data(), replay_file)) { + ++nr_published_messages; + } + + nextDataMessage(replay_file, sub, next_msg_id); + + // TODO: output status (eg. every sec), including total duration... + } + + for (auto &subscription : _subscriptions) { + if (!subscription) { + continue; + } + + if (subscription->compat) { + delete subscription->compat; + subscription->compat = nullptr; + } + + if (subscription->orb_advert) { + orb_unadvertise(subscription->orb_advert); + subscription->orb_advert = nullptr; + } + } + + if (!should_exit()) { + PX4_INFO("Replay done (published %u msgs, %.3lf s)", nr_published_messages, + (double)hrt_elapsed_time(&_replay_start_time) / 1.e6); + } + + onExitMainLoop(); + + if (!should_exit()) { + replay_file.close(); + px4_shutdown_request(); + // we need to ensure the shutdown logic gets updated and eventually triggers shutdown + hrt_abstime t = hrt_absolute_time(); + + for (int i = 0; i < 1000; ++i) { + struct timespec ts; + abstime_to_ts(&ts, t); + px4_clock_settime(CLOCK_MONOTONIC, &ts); + t += 10_ms; + } + } +} + +void +Replay::readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_file) +{ + const size_t msg_read_size = sub.orb_meta->o_size_no_padding; + const size_t msg_write_size = sub.orb_meta->o_size; + _read_buffer.reserve(msg_write_size); + replay_file.seekg(sub.next_read_pos + (streamoff)(ULOG_MSG_HEADER_LEN + 2)); //skip header & msg id + replay_file.read((char *)_read_buffer.data(), msg_read_size); +} + +bool +Replay::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) +{ + return publishTopic(sub, data); +} + +uint64_t +Replay::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset) +{ + const uint64_t publish_timestamp = next_file_time + timestamp_offset; + + // wait if necessary + uint64_t cur_time = hrt_absolute_time(); + + // if some topics have a timestamp smaller than the log file start, publish them immediately + if (cur_time < publish_timestamp && next_file_time > _file_start_time) { + if (_speed_factor > FLT_EPSILON) { + // avoid many small usleep calls + _accumulated_delay += (publish_timestamp - cur_time) / _speed_factor; + + if (_accumulated_delay > 3000) { + system_usleep(_accumulated_delay); + _accumulated_delay = 0.f; + } + } + + // adjust the lockstep time to the publication time + struct timespec ts; + abstime_to_ts(&ts, publish_timestamp); + px4_clock_settime(CLOCK_MONOTONIC, &ts); + } + + return publish_timestamp; +} + +bool +Replay::publishTopic(Subscription &sub, void *data) +{ + bool published = false; + + if (sub.compat) { + data = sub.compat->apply(data); + } + + if (sub.orb_advert) { + orb_publish(sub.orb_meta, sub.orb_advert, data); + published = true; + + } else { + if (sub.multi_id == 0) { + sub.orb_advert = orb_advertise(sub.orb_meta, data); + published = true; + + } else { + // make sure the other instances are advertised already so that we get the correct instance + bool advertised = false; + + for (const auto &subscription : _subscriptions) { + if (!subscription) { + continue; + } + + if (subscription->orb_meta) { + if (strcmp(sub.orb_meta->o_name, subscription->orb_meta->o_name) == 0 && + subscription->orb_advert && subscription->multi_id == sub.multi_id - 1) { + advertised = true; + } + } + } + + if (advertised) { + int instance; + sub.orb_advert = orb_advertise_multi(sub.orb_meta, data, &instance); + published = true; + } + } + } + + if (published) { + ++sub.publication_counter; + } + + return published; +} + +int +Replay::custom_command(int argc, char *argv[]) +{ + if (!strcmp(argv[0], "tryapplyparams")) { + return Replay::applyParams(true); + } + + if (!strcmp(argv[0], "trystart")) { + return Replay::task_spawn(argc, argv); + } + + return print_usage("unknown command"); +} + +int +Replay::task_spawn(int argc, char *argv[]) +{ + // check if a log file was found + if (!isSetup()) { + if (argc > 0 && strncmp(argv[0], "try", 3) == 0) { + return 0; + } + + PX4_ERR("no log file given (via env variable %s)", replay::ENV_FILENAME); + return -1; + } + + _task_id = px4_task_spawn_cmd("replay", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4000, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +int +Replay::applyParams(bool quiet) +{ + if (!isSetup()) { + if (quiet) { + return -1; + } + + PX4_ERR("no log file given (via env variable %s)", replay::ENV_FILENAME); + return -1; + } + + int ret = 0; + Replay *r = new Replay(); + + if (r == nullptr) { + PX4_ERR("alloc failed"); + return -ENOMEM; + } + + ifstream replay_file(_replay_file, ios::in | ios::binary); + + if (!r->readDefinitionsAndApplyParams(replay_file)) { + ret = -1; + } + + delete r; + + return ret; +} + +Replay * +Replay::instantiate(int argc, char *argv[]) +{ + // check the replay mode + const char *replay_mode = getenv(replay::ENV_MODE); + + Replay *instance = nullptr; + + if (replay_mode && strcmp(replay_mode, "ekf2") == 0) { + PX4_INFO("Ekf2 replay mode"); + instance = new ReplayEkf2(); + + } else { + instance = new Replay(); + } + + return instance; +} + +int +Replay::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is used to replay ULog files. + +There are 2 environment variables used for configuration: `replay`, which must be set to an ULog file name - it's +the log file to be replayed. The second is the mode, specified via `replay_mode`: +- `replay_mode=ekf2`: specific EKF2 replay mode. It can only be used with the ekf2 module, but allows the replay + to run as fast as possible. +- Generic otherwise: this can be used to replay any module(s), but the replay will be done with the same speed as the + log was recorded. + +The module is typically used together with uORB publisher rules, to specify which messages should be replayed. +The replay module will just publish all messages that are found in the log. It also applies the parameters from +the log. + +The replay procedure is documented on the [System-wide Replay](https://dev.px4.io/master/en/debug/system_wide_replay.html) +page. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("replay", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start replay, using log file from ENV variable 'replay'"); + PRINT_MODULE_USAGE_COMMAND_DESCR("trystart", "Same as 'start', but silently exit if no log file given"); + PRINT_MODULE_USAGE_COMMAND_DESCR("tryapplyparams", "Try to apply the parameters from the log file"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/replay/Replay.hpp b/src/prometheus_px4/src/modules/replay/Replay.hpp new file mode 100644 index 0000000..6b52c12 --- /dev/null +++ b/src/prometheus_px4/src/modules/replay/Replay.hpp @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 +#include +#include +#include +#include + +#include "definitions.hpp" + +#include +#include +#include + +namespace px4 +{ + +/** + * @class Replay + * Parses an ULog file and replays it in 'real-time'. The timestamp of each replayed message is offset + * to match the starting time of replay. It keeps a stream for each subscription to find the next message + * to replay. This is necessary because data messages from different subscriptions don't need to be in + * monotonic increasing order. + */ +class Replay : public ModuleBase +{ +public: + Replay() = default; + + virtual ~Replay(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static Replay *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; + + /** + * Apply the parameters from the log + * @param quiet do not print an error if true and no log file given via ENV + * @return 0 on success + */ + static int applyParams(bool quiet); + + /** + * Tell the replay module that we want to use replay mode. + * After that, only 'replay start' must be executed (typically the last step after startup). + * @param file_name file name of the used log replay file. Will be copied. + */ + static void setupReplayFile(const char *file_name); + + static bool isSetup() { return _replay_file; } + +protected: + + /** + * @class Compatibility base class to convert topics to an updated format + */ + class CompatBase + { + public: + virtual ~CompatBase() = default; + + /** + * apply compatibility to a topic + * @param data input topic (can be modified in place) + * @return new topic data + */ + virtual void *apply(void *data) = 0; + }; + + class CompatSensorCombinedDtType : public CompatBase + { + public: + CompatSensorCombinedDtType(int gyro_integral_dt_offset_log, int gyro_integral_dt_offset_intern, + int accelerometer_integral_dt_offset_log, int accelerometer_integral_dt_offset_intern); + + void *apply(void *data) override; + private: + int _gyro_integral_dt_offset_log; + int _gyro_integral_dt_offset_intern; + int _accelerometer_integral_dt_offset_log; + int _accelerometer_integral_dt_offset_intern; + }; + + struct Subscription { + + const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid + orb_advert_t orb_advert = nullptr; + uint8_t multi_id; + int timestamp_offset; ///< marks the field of the timestamp + + bool ignored = false; ///< if true, it will not be considered for publication in the main loop + + std::streampos next_read_pos; + uint64_t next_timestamp; ///< timestamp of the file + + CompatBase *compat = nullptr; + + // statistics + int error_counter = 0; + int publication_counter = 0; + }; + + /** + * Find the offset & field size in bytes for a given field name + * @param format format string, as specified by ULog + * @param field_name search for this field + * @param offset returned offset + * @param field_size returned field size + * @return true if found, false otherwise + */ + static bool findFieldOffset(const std::string &format, const std::string &field_name, int &offset, int &field_size); + + /** + * publish an orb topic + * @param sub + * @param data + * @return true if published, false otherwise + */ + bool publishTopic(Subscription &sub, void *data); + + /** + * called when entering the main replay loop + */ + virtual void onEnterMainLoop() {} + + /** + * called when exiting the main replay loop + */ + virtual void onExitMainLoop() {} + + /** + * called when a new subscription is added + */ + virtual void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {} + + /** + * handle delay until topic can be published. + * @param next_file_timestamp timestamp of next message to publish + * @param timestamp_offset offset between file start time and replay start time + * @return timestamp that the message to publish should have + */ + virtual uint64_t handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset); + + /** + * handle the publication of a topic update + * @return true if published, false otherwise + */ + virtual bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file); + + /** + * read a topic from the file (offset given by the subscription) into _read_buffer + */ + void readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_file); + + /** + * Find next data message for this subscription, starting with the stored file offset. + * Skip the first message, and if found, read the timestamp and store the new file offset. + * This also takes care of new subscriptions and parameter updates. When reaching EOF, + * the subscription is set to invalid. + * File seek position is arbitrary after this call. + * @return false on file error + */ + bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id); + + virtual uint64_t getTimestampOffset() + { + //we update the timestamps from the file by a constant offset to match + //the current replay time + return _replay_start_time - _file_start_time; + } + + std::vector _subscriptions; + std::vector _read_buffer; + + float _speed_factor{1.f}; ///< from PX4_SIM_SPEED_FACTOR env variable (set to 0 to avoid usleep = unlimited rate) + +private: + std::set _overridden_params; + std::map _file_formats; ///< all formats we read from the file + + uint64_t _file_start_time; + uint64_t _replay_start_time; + std::streampos _data_section_start; ///< first ADD_LOGGED_MSG message + + /** keep track of file position to avoid adding a subscription multiple times. */ + std::streampos _subscription_file_pos = 0; + + int64_t _read_until_file_position = 1ULL << 60; ///< read limit if log contains appended data + + float _accumulated_delay{0.f}; + + bool readFileHeader(std::ifstream &file); + + /** + * Read definitions section: check formats, apply parameters and store + * the start of the data section. + * @return true on success + */ + bool readFileDefinitions(std::ifstream &file); + + ///file parsing methods. They return false, when further parsing should be aborted. + bool readFormat(std::ifstream &file, uint16_t msg_size); + bool readAndAddSubscription(std::ifstream &file, uint16_t msg_size); + bool readFlagBits(std::ifstream &file, uint16_t msg_size); + + /** + * Read the file header and definitions sections. Apply the parameters from this section + * and apply user-defined overridden parameters. + * @return true on success + */ + bool readDefinitionsAndApplyParams(std::ifstream &file); + + /** + * Read and handle additional messages starting at current file position, while position < end_position. + * This handles dropout and parameter update messages. + * We need to handle these separately, because they have no timestamp. We look at the file position instead. + * @return false on file error + */ + bool readAndHandleAdditionalMessages(std::ifstream &file, std::streampos end_position); + bool readDropout(std::ifstream &file, uint16_t msg_size); + bool readAndApplyParameter(std::ifstream &file, uint16_t msg_size); + + static const orb_metadata *findTopic(const std::string &name); + + /** get the array size from a type. eg. float[3] -> return float */ + static std::string extractArraySize(const std::string &type_name_full, int &array_size); + + /** get the size of a type that is not an array */ + static size_t sizeOfType(const std::string &type_name); + + /** get the size of a type that can be an array */ + static size_t sizeOfFullType(const std::string &type_name_full); + + void setUserParams(const char *filename); + + static char *_replay_file; +}; + +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/replay/ReplayEkf2.cpp b/src/prometheus_px4/src/modules/replay/ReplayEkf2.cpp new file mode 100644 index 0000000..9417183 --- /dev/null +++ b/src/prometheus_px4/src/modules/replay/ReplayEkf2.cpp @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 +#include +#include + +// for ekf2 replay +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ReplayEkf2.hpp" + +namespace px4 +{ + +bool +ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) +{ + if (sub.orb_meta == ORB_ID(ekf2_timestamps)) { + ekf2_timestamps_s ekf2_timestamps; + memcpy(&ekf2_timestamps, data, sub.orb_meta->o_size); + + if (!publishEkf2Topics(ekf2_timestamps, replay_file)) { + return false; + } + + // Wait for modules to process the data + px4_lockstep_wait_for_components(); + + return true; + + } else if (sub.orb_meta == ORB_ID(vehicle_status) || sub.orb_meta == ORB_ID(vehicle_land_detected) + || sub.orb_meta == ORB_ID(vehicle_gps_position)) { + return publishTopic(sub, data); + } // else: do not publish + + return false; +} + +void +ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) +{ + if (sub.orb_meta == ORB_ID(sensor_combined)) { + _sensor_combined_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(airspeed)) { + _airspeed_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(distance_sensor)) { + _distance_sensor_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(optical_flow)) { + _optical_flow_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_air_data)) { + _vehicle_air_data_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_magnetometer)) { + _vehicle_magnetometer_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_visual_odometry)) { + _vehicle_visual_odometry_msg_id = msg_id; + } + + // the main loop should only handle publication of the following topics, the sensor topics are + // handled separately in publishEkf2Topics() + // Note: the GPS is not treated here since not missing data is more important than the accuracy of the timestamp + sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status) + && sub.orb_meta != ORB_ID(vehicle_land_detected) && sub.orb_meta != ORB_ID(vehicle_gps_position); +} + +bool +ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file) +{ + auto handle_sensor_publication = [&](int16_t timestamp_relative, uint16_t msg_id) { + if (timestamp_relative != ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID) { + // timestamp_relative is already given in 0.1 ms + uint64_t t = timestamp_relative + ekf2_timestamps.timestamp / 100; // in 0.1 ms + findTimestampAndPublish(t, msg_id, replay_file); + } + }; + + handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id); + handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id); + handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id); + handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id); + handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id); + handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id); + + // sensor_combined: publish last because ekf2 is polling on this + if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) { + if (_sensor_combined_msg_id == msg_id_invalid) { + // subscription not found yet or sensor_combined not contained in log + return false; + + } else if (!_subscriptions[_sensor_combined_msg_id]->orb_meta) { + return false; // read past end of file + + } else { + // we should publish a topic, just publish the same again + readTopicDataToBuffer(*_subscriptions[_sensor_combined_msg_id], replay_file); + publishTopic(*_subscriptions[_sensor_combined_msg_id], _read_buffer.data()); + } + } + + return true; +} + +bool +ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file) +{ + if (msg_id == msg_id_invalid) { + // could happen if a topic is not logged + return false; + } + + Subscription &sub = *_subscriptions[msg_id]; + + while (sub.next_timestamp / 100 < timestamp && sub.orb_meta) { + nextDataMessage(replay_file, sub, msg_id); + } + + if (!sub.orb_meta) { // no messages anymore + return false; + } + + if (sub.next_timestamp / 100 != timestamp) { + // this can happen in beginning of the log or on a dropout + PX4_DEBUG("No timestamp match found for topic %s (%i, %i)", sub.orb_meta->o_name, (int)sub.next_timestamp / 100, + timestamp); + ++sub.error_counter; + return false; + } + + readTopicDataToBuffer(sub, replay_file); + publishTopic(sub, _read_buffer.data()); + return true; +} + +void +ReplayEkf2::onEnterMainLoop() +{ + _speed_factor = 0.f; // iterate as fast as possible +} + +void +ReplayEkf2::onExitMainLoop() +{ + // print statistics + auto print_sensor_statistics = [this](uint16_t msg_id, const char *name) { + if (msg_id != msg_id_invalid) { + Subscription &sub = *_subscriptions[msg_id]; + + if (sub.publication_counter > 0 || sub.error_counter > 0) { + PX4_INFO("%s: %i, %i", name, sub.publication_counter, sub.error_counter); + } + } + }; + + PX4_INFO(""); + PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):"); + + print_sensor_statistics(_airspeed_msg_id, "airspeed"); + print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor"); + print_sensor_statistics(_optical_flow_msg_id, "optical_flow"); + print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined"); + print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data"); + print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer"); + print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry"); +} + +} // namespace px4 diff --git a/src/prometheus_px4/src/modules/replay/ReplayEkf2.hpp b/src/prometheus_px4/src/modules/replay/ReplayEkf2.hpp new file mode 100644 index 0000000..43c7de1 --- /dev/null +++ b/src/prometheus_px4/src/modules/replay/ReplayEkf2.hpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 "Replay.hpp" + +namespace px4 +{ + +/** + * @class ReplayEkf2 + * replay specialization for Ekf2 replay + */ +class ReplayEkf2 : public Replay +{ +public: +protected: + + void onEnterMainLoop() override; + void onExitMainLoop() override; + + /** + * handle ekf2 topic publication in ekf2 replay mode + * @param sub + * @param data + * @param replay_file file currently replayed (file seek position should be considered arbitrary after this call) + * @return true if published, false otherwise + */ + bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override; + + void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) override; + + uint64_t getTimestampOffset() override + { + // avoid offsetting timestamps as we use them to compare against the log + return 0; + } +private: + + bool publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file); + + /** + * find the next message for a subscription that matches a given timestamp and publish it + * @param timestamp in 0.1 ms + * @param msg_id + * @param replay_file file currently replayed (file seek position should be considered arbitrary after this call) + * @return true if timestamp found and published + */ + bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file); + + static constexpr uint16_t msg_id_invalid = 0xffff; + + uint16_t _airspeed_msg_id = msg_id_invalid; + uint16_t _distance_sensor_msg_id = msg_id_invalid; + uint16_t _optical_flow_msg_id = msg_id_invalid; + uint16_t _sensor_combined_msg_id = msg_id_invalid; + uint16_t _vehicle_air_data_msg_id = msg_id_invalid; + uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid; + uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid; +}; + +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/replay/definitions.hpp b/src/prometheus_px4/src/modules/replay/definitions.hpp new file mode 100644 index 0000000..100a5e6 --- /dev/null +++ b/src/prometheus_px4/src/modules/replay/definitions.hpp @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 + +namespace px4 +{ +namespace replay +{ + +static const char __attribute__((unused)) *ENV_FILENAME = "replay"; ///< name for getenv() +static const char __attribute__((unused)) *ENV_MODE = "replay_mode"; ///< name for getenv() + + +} //namespace replay +} //namespace px4 diff --git a/src/prometheus_px4/src/modules/replay/replay_main.cpp b/src/prometheus_px4/src/modules/replay/replay_main.cpp new file mode 100644 index 0000000..9f18eb2 --- /dev/null +++ b/src/prometheus_px4/src/modules/replay/replay_main.cpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 "Replay.hpp" + +using namespace px4; + +extern "C" __EXPORT int +replay_main(int argc, char *argv[]) +{ + //check for logfile env variable + const char *logfile = getenv(replay::ENV_FILENAME); + + if (logfile && !Replay::isSetup()) { + PX4_INFO("using replay log file: %s", logfile); + Replay::setupReplayFile(logfile); + } + + return Replay::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/rover_pos_control/CMakeLists.txt b/src/prometheus_px4/src/modules/rover_pos_control/CMakeLists.txt new file mode 100644 index 0000000..5e09386 --- /dev/null +++ b/src/prometheus_px4/src/modules/rover_pos_control/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ +px4_add_module( + MODULE modules__rover_pos_control + MAIN rover_pos_control + SRCS + RoverPositionControl.cpp + RoverPositionControl.hpp + DEPENDS + l1 + pid + ) diff --git a/src/prometheus_px4/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/prometheus_px4/src/modules/rover_pos_control/RoverPositionControl.cpp new file mode 100644 index 0000000..eac2e89 --- /dev/null +++ b/src/prometheus_px4/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -0,0 +1,569 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the fixed wing module and it is designed for ground rovers. + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Marco Zorzi + */ + + +#include "RoverPositionControl.hpp" +#include + +#define ACTUATOR_PUBLISH_PERIOD_MS 4 + +using namespace matrix; + +/** + * L1 control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int rover_pos_control_main(int argc, char *argv[]); + +RoverPositionControl::RoverPositionControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) // TODO : do we even need these perf counters +{ +} + +RoverPositionControl::~RoverPositionControl() +{ + perf_free(_loop_perf); +} + +bool +RoverPositionControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle angular velocity callback registration failed!"); + return false; + } + + return true; +} + +void RoverPositionControl::parameters_update(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(); + + _gnd_control.set_l1_damping(_param_l1_damping.get()); + _gnd_control.set_l1_period(_param_l1_period.get()); + _gnd_control.set_l1_roll_limit(math::radians(0.0f)); + + pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f); + pid_set_parameters(&_speed_ctrl, + _param_speed_p.get(), + _param_speed_i.get(), + _param_speed_d.get(), + _param_speed_imax.get(), + _param_gndspeed_max.get()); + } +} + +void +RoverPositionControl::vehicle_control_mode_poll() +{ + if (_control_mode_sub.updated()) { + _control_mode_sub.copy(&_control_mode); + } +} + +void +RoverPositionControl::manual_control_setpoint_poll() +{ + if (_control_mode.flag_control_manual_enabled) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f, 0.0002f, 0.04f); + + if (!_control_mode.flag_control_climb_rate_enabled && + !_control_mode.flag_control_offboard_enabled) { + + if (_control_mode.flag_control_attitude_enabled) { + // STABILIZED mode generate the attitude setpoint from manual user inputs + _att_sp.roll_body = 0.0; + _att_sp.pitch_body = 0.0; + + /* reset yaw setpoint to current position if needed */ + if (_reset_yaw_sp) { + const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); + _manual_yaw_sp = vehicle_yaw; + _reset_yaw_sp = false; + + } else { + const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.y * yaw_rate; + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); + } + + _att_sp.yaw_body = _manual_yaw_sp; + _att_sp.thrust_body[0] = _manual_control_setpoint.z; + + Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + q.copyTo(_att_sp.q_d); + + _att_sp.timestamp = hrt_absolute_time(); + + + _attitude_sp_pub.publish(_att_sp); + + } else { + _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y; + _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x; + // Set heading from the manual roll input channel + _act_controls.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r; + // Set throttle from the manual throttle channel + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; + _reset_yaw_sp = true; + } + + } else { + _reset_yaw_sp = true; + } + + _manual_setpoint_last_called = hrt_absolute_time(); + } + } +} + +void +RoverPositionControl::position_setpoint_triplet_poll() +{ + if (_pos_sp_triplet_sub.updated()) { + _pos_sp_triplet_sub.copy(&_pos_sp_triplet); + } +} + +void +RoverPositionControl::attitude_setpoint_poll() +{ + if (_att_sp_sub.updated()) { + _att_sp_sub.copy(&_att_sp); + } +} + +void +RoverPositionControl::vehicle_attitude_poll() +{ + if (_att_sub.updated()) { + _att_sub.copy(&_vehicle_att); + } +} + +bool +RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, + const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) +{ + float dt = 0.01; // Using non zero value to a avoid division by zero + + if (_control_position_last_called > 0) { + dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f; + } + + _control_position_last_called = hrt_absolute_time(); + + bool setpoint = true; + + if ((_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_offboard_enabled) && pos_sp_triplet.current.valid) { + /* AUTONOMOUS FLIGHT */ + + _control_mode_current = UGV_POSCTRL_MODE_AUTO; + + /* get circle mode */ + //bool was_circle_mode = _gnd_control.circle_mode(); + + /* current waypoint (the one currently heading for) */ + matrix::Vector2d curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + + /* previous waypoint */ + matrix::Vector2d prev_wp = curr_wp; + + if (pos_sp_triplet.previous.valid) { + prev_wp(0) = pos_sp_triplet.previous.lat; + prev_wp(1) = pos_sp_triplet.previous.lon; + } + + matrix::Vector2f ground_speed_2d(ground_speed); + + float mission_throttle = _param_throttle_cruise.get(); + + /* Just control the throttle */ + if (_param_speed_control_mode.get() == 1) { + /* control the speed in closed loop */ + + float mission_target_speed = _param_gndspeed_trim.get(); + + if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && + _pos_sp_triplet.current.cruising_speed > 0.1f) { + mission_target_speed = _pos_sp_triplet.current.cruising_speed; + } + + // Velocity in body frame + const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); + const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2)); + + const float x_vel = vel(0); + const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; + + // Compute airspeed control out and just scale it as a constant + mission_throttle = _param_throttle_speed_scaler.get() + * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt); + + // Constrain throttle between min and max + mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get()); + + } else { + /* Just control throttle in open loop */ + if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) && + _pos_sp_triplet.current.cruising_throttle > 0.01f) { + + mission_throttle = _pos_sp_triplet.current.cruising_throttle; + } + } + + float dist_target = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + (double)curr_wp(0), (double)curr_wp(1)); // pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + + //PX4_INFO("Setpoint type %d", (int) pos_sp_triplet.current.type ); + //PX4_INFO(" State machine state %d", (int) _pos_ctrl_state); + //PX4_INFO(" Setpoint Lat %f, Lon %f", (double) curr_wp(0), (double)curr_wp(1)); + //PX4_INFO(" Distance to target %f", (double) dist_target); + + switch (_pos_ctrl_state) { + case GOTO_WAYPOINT: { + if (dist_target < _param_nav_loiter_rad.get()) { + _pos_ctrl_state = STOPPING; // We are closer than loiter radius to waypoint, stop. + + } else { + _gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle; + + float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand()); + float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get()); + float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign( + _gnd_control.nav_lateral_acceleration_demand()); + control_effort = math::constrain(control_effort, -1.0f, 1.0f); + _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + } + } + break; + + case STOPPING: { + _act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f; + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + // Note _prev_wp is different to the local prev_wp which is related to a mission waypoint. + float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1), + (double)curr_wp(0), (double)curr_wp(1)); + + if (dist_between_waypoints > 0) { + _pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it + } + + //PX4_INFO(" Distance between prev and curr waypoints %f", (double)dist_between_waypoints); + } + break; + + default: + PX4_ERR("Unknown Rover State"); + _pos_ctrl_state = STOPPING; + break; + } + + _prev_wp = curr_wp; + + } else { + _control_mode_current = UGV_POSCTRL_MODE_OTHER; + setpoint = false; + } + + return setpoint; +} + +void +RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) +{ + const Vector3f desired_velocity{_trajectory_setpoint.vx, _trajectory_setpoint.vy, _trajectory_setpoint.vz}; + float dt = 0.01; // Using non zero value to a avoid division by zero + + const float mission_throttle = _param_throttle_cruise.get(); + const float desired_speed = desired_velocity.norm(); + + if (desired_speed > 0.01f) { + const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); + const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2)); + + const float x_vel = vel(0); + const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; + + const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt); + + //Constrain maximum throttle to mission throttle + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, mission_throttle); + + Vector3f desired_body_velocity; + + if (_velocity_frame == VelocityFrame::NED) { + desired_body_velocity = desired_velocity; + + } else { + // If the frame of the velocity setpoint is unknown, assume it is in local frame + desired_body_velocity = R_to_body * desired_velocity; + } + + const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0)); + float control_effort = desired_theta / _param_max_turn_angle.get(); + control_effort = math::constrain(control_effort, -1.0f, 1.0f); + + _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + + } else { + + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + _act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f; + } +} + +void +RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp) +{ + // quaternion attitude control law, qe is rotation from q to qd + const Quatf qe = Quatf(att.q).inversed() * Quatf(att_sp.q_d); + const Eulerf euler_sp = qe; + + float control_effort = euler_sp(2) / _param_max_turn_angle.get(); + control_effort = math::constrain(control_effort, -1.0f, 1.0f); + + _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + + const float control_throttle = att_sp.thrust_body[0]; + + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f); + +} + +void +RoverPositionControl::Run() +{ + parameters_update(true); + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + + /* check vehicle control mode for changes to publication state */ + vehicle_control_mode_poll(); + attitude_setpoint_poll(); + vehicle_attitude_poll(); + manual_control_setpoint_poll(); + + _vehicle_acceleration_sub.update(); + + /* update parameters from storage */ + parameters_update(); + + /* only run controller if position changed */ + if (_local_pos_sub.update(&_local_pos)) { + + /* load local copies */ + _global_pos_sub.update(&_global_pos); + + position_setpoint_triplet_poll(); + + // Convert Local setpoints to global setpoints + if (_control_mode.flag_control_offboard_enabled) { + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) { + + map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_timestamp); + + _global_local_alt0 = _local_pos.ref_alt; + } + + _trajectory_setpoint_sub.update(&_trajectory_setpoint); + + // local -> global + map_projection_reproject(&_global_local_proj_ref, + _trajectory_setpoint.x, _trajectory_setpoint.y, + &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon); + + _pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.z; + _pos_sp_triplet.current.valid = true; + } + + // update the reset counters in any case + _pos_reset_counter = _global_pos.lat_lon_reset_counter; + + matrix::Vector3f ground_speed(_local_pos.vx, _local_pos.vy, _local_pos.vz); + matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon); + matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz); + + if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { + + if (control_position(current_position, ground_speed, _pos_sp_triplet)) { + + //TODO: check if radius makes sense here + float turn_distance = _param_l1_distance.get(); //_gnd_control.switch_distance(100.0f); + + // publish status + position_controller_status_s pos_ctrl_status{}; + + pos_ctrl_status.nav_roll = 0.0f; + pos_ctrl_status.nav_pitch = 0.0f; + pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing(); + + pos_ctrl_status.target_bearing = _gnd_control.target_bearing(); + pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error(); + + pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); + + pos_ctrl_status.acceptance_radius = turn_distance; + pos_ctrl_status.yaw_acceptance = NAN; + + pos_ctrl_status.timestamp = hrt_absolute_time(); + + _pos_ctrl_status_pub.publish(pos_ctrl_status); + } + + } else if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_velocity_enabled) { + _trajectory_setpoint_sub.update(&_trajectory_setpoint); + control_velocity(current_velocity); + } + } + + // Respond to an attitude update and run the attitude controller if enabled + if (_control_mode.flag_control_attitude_enabled + && !_control_mode.flag_control_position_enabled + && !_control_mode.flag_control_velocity_enabled) { + control_attitude(_vehicle_att, _att_sp); + + } + + /* Only publish if any of the proper modes are enabled */ + if (_control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_attitude_enabled || + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_manual_enabled) { + // timestamp and publish controls + _act_controls.timestamp = hrt_absolute_time(); + _actuator_controls_pub.publish(_act_controls); + } + } +} + +int RoverPositionControl::task_spawn(int argc, char *argv[]) +{ + RoverPositionControl *instance = new RoverPositionControl(); + + 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 RoverPositionControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int RoverPositionControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Controls the position of a ground rover using an L1 controller. + +Publishes `actuator_controls_0` messages at IMU_GYRO_RATEMAX. + +### Implementation +Currently, this implementation supports only a few modes: + + * Full manual: Throttle and yaw controls are passed directly through to the actuators + * Auto mission: The rover runs missions + * Loiter: The rover will navigate to within the loiter radius, then stop the motors + +### Examples +CLI usage example: +$ rover_pos_control start +$ rover_pos_control status +$ rover_pos_control stop + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_pos_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start") + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int rover_pos_control_main(int argc, char *argv[]) +{ + return RoverPositionControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/prometheus_px4/src/modules/rover_pos_control/RoverPositionControl.hpp new file mode 100644 index 0000000..4be4536 --- /dev/null +++ b/src/prometheus_px4/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the fixed wing module and it is designed for ground rovers. + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the ackowledgments and credits for the fw wing app are reported in those files. + * + * @author Marco Zorzi + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Dcmf; + +using namespace time_literals; + +class RoverPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + RoverPositionControl(); + ~RoverPositionControl(); + RoverPositionControl(const RoverPositionControl &) = delete; + RoverPositionControl operator=(const RoverPositionControl &other) = delete; + + /** @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 _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Publication _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */ + uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */ + + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< control mode subscription */ + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + + manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ + position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ + vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */ + vehicle_control_mode_s _control_mode{}; /**< control mode */ + vehicle_global_position_s _global_pos{}; /**< global vehicle position */ + vehicle_local_position_s _local_pos{}; /**< global vehicle position */ + actuator_controls_s _act_controls{}; /**< direct control of actuators */ + vehicle_attitude_s _vehicle_att{}; + vehicle_local_position_setpoint_s _trajectory_setpoint{}; + + uORB::SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + hrt_abstime _control_position_last_called{0}; /**) _param_l1_period, + (ParamFloat) _param_l1_damping, + (ParamFloat) _param_l1_distance, + + (ParamFloat) _param_gndspeed_trim, + (ParamFloat) _param_gndspeed_max, + + (ParamInt) _param_speed_control_mode, + (ParamFloat) _param_speed_p, + (ParamFloat) _param_speed_i, + (ParamFloat) _param_speed_d, + (ParamFloat) _param_speed_imax, + (ParamFloat) _param_throttle_speed_scaler, + + (ParamFloat) _param_throttle_min, + (ParamFloat) _param_throttle_max, + (ParamFloat) _param_throttle_cruise, + + (ParamFloat) _param_wheel_base, + (ParamFloat) _param_max_turn_angle, + (ParamFloat) _param_gnd_man_y_max, + (ParamFloat) _param_nav_loiter_rad /**< loiter radius for Rover */ + ) + + /** + * Update our local parameter cache. + */ + void parameters_update(bool force = false); + + void position_setpoint_triplet_poll(); + void attitude_setpoint_poll(); + void vehicle_control_mode_poll(); + void vehicle_attitude_poll(); + void manual_control_setpoint_poll(); + + /** + * Control position. + */ + bool control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed, + const position_setpoint_triplet_s &_pos_sp_triplet); + void control_velocity(const matrix::Vector3f ¤t_velocity); + void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp); + +}; diff --git a/src/prometheus_px4/src/modules/rover_pos_control/rover_pos_control_params.c b/src/prometheus_px4/src/modules/rover_pos_control/rover_pos_control_params.c new file mode 100644 index 0000000..6458fb1 --- /dev/null +++ b/src/prometheus_px4/src/modules/rover_pos_control/rover_pos_control_params.c @@ -0,0 +1,287 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 rover_pos_control_params.c + * + * Parameters defined by the position control task for ground rovers + * + * This is a modification of the fixed wing params and it is designed for ground rovers. + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the ackowledgments and credits for the fw wing app are reported in those files. + * + * @author Marco Zorzi + */ + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * Distance from front axle to rear axle + * + * A value of 0.31 is typical for 1/10 RC cars. + * + * @unit m + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f); + +/** + * L1 distance + * + * This is the distance at which the next waypoint is activated. This should be set + * to about 2-4x of GND_WHEEL_BASE and not smaller than one meter (due to GPS accuracy). + * + * + * @unit m + * @min 1.0 + * @max 50.0 + * @decimal 1 + * @increment 0.1 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f); + +/** + * L1 period + * + * This is the L1 distance and defines the tracking + * point ahead of the rover it's following. + * Use values around 2-5m for a 0.3m wheel base. Tuning instructions: Shorten + * slowly during tuning until response is sharp without oscillation. + * + * @unit m + * @min 0.5 + * @max 50.0 + * @decimal 1 + * @increment 0.5 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f); + +/** + * L1 damping + * + * Damping factor for L1 control. + * + * @min 0.6 + * @max 0.9 + * @decimal 2 + * @increment 0.05 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f); + +/** + * Cruise throttle + * + * This is the throttle setting required to achieve the desired cruise speed. + * 10% is ok for a traxxas stampede vxl with ESC set to training mode + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f); + +/** + * Throttle limit max + * + * This is the maximum throttle % that can be used by the controller. + * For a Traxxas stampede vxl with the ESC set to training, 30 % is enough + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f); + +/** + * Throttle limit min + * + * This is the minimum throttle % that can be used by the controller. + * Set to 0 for rover + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f); + +/** + * Control mode for speed + * + * This allows the user to choose between closed loop gps speed or open loop cruise throttle speed + * @min 0 + * @max 1 + * @value 0 open loop control + * @value 1 close the loop with gps speed + * @group Rover Position Control + */ +PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1); + +/** + * Speed proportional gain + * + * This is the proportional gain for the speed closed loop controller + * + * @unit %m/s + * @min 0.005 + * @max 50.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f); + +/** + * Speed Integral gain + * + * This is the integral gain for the speed closed loop controller + * + * @unit %m/s + * @min 0.00 + * @max 50.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_SPEED_I, 3.0f); + +/** + * Speed proportional gain + * + * This is the derivative gain for the speed closed loop controller + * + * @unit %m/s + * @min 0.00 + * @max 50.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.001f); + +/** + * Speed integral maximum value + * + * This is the maxim value the integral can reach to prevent wind-up. + * + * @unit %m/s + * @min 0.005 + * @max 50.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f); + +/** + * Speed to throttle scaler + * + * This is a gain to map the speed control output to the throttle linearly. + * + * @unit %m/s + * @min 0.005 + * @max 50.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f); + +/** + * Trim ground speed + * + * + * @unit m/s + * @min 0.0 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); + +/** + * Maximum ground speed + * + * + * @unit m/s + * @min 0.0 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); + +/** + * Maximum turn angle for Ackerman steering. + * + * At a control output of 0, the steering wheels are at 0 radians. + * At a control output of 1, the steering wheels are at GND_MAX_ANG radians. + * + * @unit rad + * @min 0.0 + * @max 3.14159 + * @decimal 3 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 400 + * @decimal 1 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f); diff --git a/src/prometheus_px4/src/modules/sensors/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/CMakeLists.txt new file mode 100644 index 0000000..e2f6053 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/CMakeLists.txt @@ -0,0 +1,64 @@ +############################################################################ +# +# Copyright (c) 2015-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(data_validator) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(vehicle_acceleration) +add_subdirectory(vehicle_angular_velocity) +add_subdirectory(vehicle_air_data) +add_subdirectory(vehicle_gps_position) +add_subdirectory(vehicle_imu) +add_subdirectory(vehicle_magnetometer) + +px4_add_module( + MODULE modules__sensors + MAIN sensors + SRCS + voted_sensors_update.cpp + sensors.cpp + DEPENDS + airspeed + conversion + data_validator + drivers__device + git_ecl + mathlib + sensor_calibration + vehicle_acceleration + vehicle_angular_velocity + vehicle_air_data + vehicle_gps_position + vehicle_imu + vehicle_magnetometer + ) diff --git a/src/prometheus_px4/src/modules/sensors/analog_battery_params_deprecated.c b/src/prometheus_px4/src/modules/sensors/analog_battery_params_deprecated.c new file mode 100644 index 0000000..7166cc3 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/analog_battery_params_deprecated.c @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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_V_DIV. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0); + +/** + * This parameter is deprecated. Please use BAT1_A_PER_V. + * + * @group Battery Calibration + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0); diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/data_validator/CMakeLists.txt new file mode 100644 index 0000000..e60b18d --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2018-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_library(data_validator + DataValidator.cpp + DataValidator.hpp + DataValidatorGroup.cpp + DataValidatorGroup.hpp +) diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/DataValidator.cpp b/src/prometheus_px4/src/modules/sensors/data_validator/DataValidator.cpp new file mode 100644 index 0000000..81871a1 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/DataValidator.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file DataValidator.cpp + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "DataValidator.hpp" + +#include + +void DataValidator::put(uint64_t timestamp, float val, uint32_t error_count_in, uint8_t priority_in) +{ + float data[dimensions] = {val}; // sets the first value and all others to 0 + put(timestamp, data, error_count_in, priority_in); +} + +void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_t error_count_in, uint8_t priority_in) +{ + + _event_count++; + + if (error_count_in > _error_count) { + _error_density += (error_count_in - _error_count); + + } else if (_error_density > 0) { + _error_density--; + } + + _error_count = error_count_in; + _priority = priority_in; + + for (unsigned i = 0; i < dimensions; i++) { + if (_time_last == 0) { + _mean[i] = 0; + _lp[i] = val[i]; + _M2[i] = 0; + + } else { + float lp_val = val[i] - _lp[i]; + + float delta_val = lp_val - _mean[i]; + _mean[i] += delta_val / _event_count; + _M2[i] += delta_val * (lp_val - _mean[i]); + _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); + + if (fabsf(_value[i] - val[i]) < 0.000001f) { + _value_equal_count++; + + } else { + _value_equal_count = 0; + } + } + + // XXX replace with better filter, make it auto-tune to update rate + _lp[i] = _lp[i] * 0.99f + 0.01f * val[i]; + + _value[i] = val[i]; + } + + _time_last = timestamp; +} + +float DataValidator::confidence(uint64_t timestamp) +{ + + float ret = 1.0f; + + /* check if we have any data */ + if (_time_last == 0) { + _error_mask |= ERROR_FLAG_NO_DATA; + ret = 0.0f; + + } else if (timestamp - _time_last > _timeout_interval) { + /* timed out - that's it */ + _error_mask |= ERROR_FLAG_TIMEOUT; + ret = 0.0f; + + } else if (_value_equal_count > _value_equal_count_threshold) { + /* we got the exact same sensor value N times in a row */ + _error_mask |= ERROR_FLAG_STALE_DATA; + ret = 0.0f; + + } else if (_error_count > NORETURN_ERRCOUNT) { + /* check error count limit */ + _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; + ret = 0.0f; + + } else if (_error_density > ERROR_DENSITY_WINDOW) { + /* cap error density counter at window size */ + _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; + _error_density = ERROR_DENSITY_WINDOW; + } + + /* no critical errors */ + if (ret > 0.0f) { + /* return local error density for last N measurements */ + ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); + + if (ret > 0.0f) { + _error_mask = ERROR_FLAG_NO_ERROR; + } + } + + return ret; +} + +void DataValidator::print() +{ + if (_time_last == 0) { + PX4_INFO("\tno data"); + return; + } + + for (unsigned i = 0; i < dimensions; i++) { + PX4_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", (double)_value[i], + (double)_lp[i], (double)_mean[i], (double)_rms[i], (double)confidence(hrt_absolute_time())); + } +} diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/DataValidator.hpp b/src/prometheus_px4/src/modules/sensors/data_validator/DataValidator.hpp new file mode 100644 index 0000000..d9f2fa8 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/DataValidator.hpp @@ -0,0 +1,200 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file DataValidator.hpp + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +class DataValidator +{ +public: + static const unsigned dimensions = 3; + + DataValidator() = default; + ~DataValidator() = default; + + /** + * Put an item into the validator. + * + * @param val Item to put + */ + void put(uint64_t timestamp, float val, uint32_t error_count, uint8_t priority); + + /** + * Put a 3D item into the validator. + * + * @param val Item to put + */ + void put(uint64_t timestamp, const float val[dimensions], uint32_t error_count, uint8_t priority); + + /** + * Get the next sibling in the group + * + * @return the next sibling + */ + DataValidator *sibling() { return _sibling; } + + /** + * Set the sibling to the next node in the group + * + */ + void setSibling(DataValidator *new_sibling) { _sibling = new_sibling; } + + /** + * Get the confidence of this validator + * @return the confidence between 0 and 1 + */ + float confidence(uint64_t timestamp); + + /** + * Get the error count of this validator + * @return the error count + */ + uint32_t error_count() const { return _error_count; } + + /** + * Get the values of this validator + * @return the stored value + */ + float *value() { return _value; } + + /** + * Get the used status of this validator + * @return true if this validator ever saw data + */ + bool used() const { return (_time_last > 0); } + + /** + * Get the priority of this validator + * @return the stored priority + */ + uint8_t priority() const { return _priority; } + + /** + * Get the error state of this validator + * @return the bitmask with the error status + */ + uint32_t state() const { return _error_mask; } + + /** + * Reset the error state of this validator + */ + void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } + + /** + * Get the RMS values of this validator + * @return the stored RMS + */ + float *rms() { return _rms; } + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint32_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + + /** + * Set the equal count threshold + * + * @param threshold The number of equal values before considering the sensor stale + */ + void set_equal_value_threshold(uint32_t threshold) { _value_equal_count_threshold = threshold; } + + /** + * Get the timeout value + * + * @return The timeout interval in microseconds + */ + uint32_t get_timeout() const { return _timeout_interval; } + + /** + * Data validator error states + */ + static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); + static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); + static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); + static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); + +private: + uint32_t _error_mask{ERROR_FLAG_NO_ERROR}; /**< sensor error state */ + + uint32_t _timeout_interval{20000}; /**< interval in which the datastream times out in us */ + + uint64_t _time_last{0}; /**< last timestamp */ + uint64_t _event_count{0}; /**< total data counter */ + uint32_t _error_count{0}; /**< error count */ + + int _error_density{0}; /**< ratio between successful reads and errors */ + + uint8_t _priority{0}; /**< sensor nominal priority */ + + float _mean[dimensions] {}; /**< mean of value */ + float _lp[dimensions] {}; /**< low pass value */ + float _M2[dimensions] {}; /**< RMS component value */ + float _rms[dimensions] {}; /**< root mean square error */ + float _value[dimensions] {}; /**< last value */ + + unsigned _value_equal_count{0}; /**< equal values in a row */ + unsigned _value_equal_count_threshold{ + VALUE_EQUAL_COUNT_DEFAULT}; /**< when to consider an equal count as a problem */ + + DataValidator *_sibling{nullptr}; /**< sibling in the group */ + + static const constexpr unsigned NORETURN_ERRCOUNT = + 10000; /**< if the error count reaches this value, return sensor as invalid */ + static const constexpr float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ + static const constexpr unsigned VALUE_EQUAL_COUNT_DEFAULT = + 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ + + /* we don't want this class to be copied */ + DataValidator(const DataValidator &) = delete; + DataValidator operator=(const DataValidator &) = delete; +}; diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/DataValidatorGroup.cpp b/src/prometheus_px4/src/modules/sensors/data_validator/DataValidatorGroup.cpp new file mode 100644 index 0000000..20c1358 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/DataValidatorGroup.cpp @@ -0,0 +1,307 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file DataValidatorGroup.cpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "DataValidatorGroup.hpp" + +#include + +#include + +DataValidatorGroup::DataValidatorGroup(unsigned siblings) +{ + + DataValidator *next = nullptr; + DataValidator *prev = nullptr; + + for (unsigned i = 0; i < siblings; i++) { + next = new DataValidator(); + + if (i == 0) { + _first = next; + + } else { + prev->setSibling(next); + } + + prev = next; + } + + _last = next; + + if (_first) { + _timeout_interval_us = _first->get_timeout(); + } +} + +DataValidatorGroup::~DataValidatorGroup() +{ + while (_first) { + DataValidator *next = _first->sibling(); + delete (_first); + _first = next; + } +} + +DataValidator *DataValidatorGroup::add_new_validator() +{ + + DataValidator *validator = new DataValidator(); + + if (!validator) { + return nullptr; + } + + _last->setSibling(validator); + _last = validator; + _last->set_timeout(_timeout_interval_us); + return _last; +} + +void DataValidatorGroup::set_timeout(uint32_t timeout_interval_us) +{ + + DataValidator *next = _first; + + while (next != nullptr) { + next->set_timeout(timeout_interval_us); + next = next->sibling(); + } + + _timeout_interval_us = timeout_interval_us; +} + +void DataValidatorGroup::set_equal_value_threshold(uint32_t threshold) +{ + + DataValidator *next = _first; + + while (next != nullptr) { + next->set_equal_value_threshold(threshold); + next = next->sibling(); + } +} + +void DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint32_t error_count, + uint8_t priority) +{ + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (i == index) { + next->put(timestamp, val, error_count, priority); + break; + } + + next = next->sibling(); + i++; + } +} + +float *DataValidatorGroup::get_best(uint64_t timestamp, int *index) +{ + + DataValidator *next = _first; + + // XXX This should eventually also include voting + int pre_check_best = _curr_best; + float pre_check_confidence = 1.0f; + int pre_check_prio = -1; + float max_confidence = -1.0f; + int max_priority = -1000; + int max_index = -1; + DataValidator *best = nullptr; + + int i = 0; + + while (next != nullptr) { + float confidence = next->confidence(timestamp); + + if (i == pre_check_best) { + pre_check_prio = next->priority(); + pre_check_confidence = confidence; + } + + /* + * Switch if: + * 1) the confidence is higher and priority is equal or higher + * 2) the confidence is less than 1% different and the priority is higher + */ + if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || + (confidence > max_confidence && (next->priority() >= max_priority)) || + (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))) && + (confidence > 0.0f)) { + max_index = i; + max_confidence = confidence; + max_priority = next->priority(); + best = next; + } + + next = next->sibling(); + i++; + } + + /* the current best sensor is not matching the previous best sensor, + * or the only sensor went bad */ + if (max_index != _curr_best || ((max_confidence < FLT_EPSILON) && (_curr_best >= 0))) { + bool true_failsafe = true; + + /* check whether the switch was a failsafe or preferring a higher priority sensor */ + if (pre_check_prio != -1 && pre_check_prio < max_priority && + fabsf(pre_check_confidence - max_confidence) < 0.1f) { + /* this is not a failover */ + true_failsafe = false; + + /* reset error flags, this is likely a hotplug sensor coming online late */ + if (best != nullptr) { + best->reset_state(); + } + } + + /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ + if (_curr_best < 0) { + _prev_best = max_index; + + } else { + /* we were initialized before, this is a real failsafe */ + _prev_best = pre_check_best; + + if (true_failsafe) { + _toggle_count++; + + /* if this is the first time, log when we failed */ + if (_first_failover_time == 0) { + _first_failover_time = timestamp; + } + } + } + + /* for all cases we want to keep a record of the best index */ + _curr_best = max_index; + } + + *index = max_index; + return (best) ? best->value() : nullptr; +} + +void DataValidatorGroup::print() +{ + PX4_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best, + (_toggle_count > 0) ? "YES" : "NO", _toggle_count); + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used()) { + uint32_t flags = next->state(); + + PX4_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : ""), + ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); + + next->print(); + } + + next = next->sibling(); + i++; + } +} + +int DataValidatorGroup::failover_index() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && + (i == (unsigned)_prev_best)) { + return i; + } + + next = next->sibling(); + i++; + } + + return -1; +} + +uint32_t DataValidatorGroup::failover_state() +{ + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && + (i == (unsigned)_prev_best)) { + return next->state(); + } + + next = next->sibling(); + i++; + } + + return DataValidator::ERROR_FLAG_NO_ERROR; +} + +uint32_t DataValidatorGroup::get_sensor_state(unsigned index) +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (i == index) { + return next->state(); + } + + next = next->sibling(); + i++; + } + + // sensor index not found + return UINT32_MAX; +} diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/DataValidatorGroup.hpp b/src/prometheus_px4/src/modules/sensors/data_validator/DataValidatorGroup.hpp new file mode 100644 index 0000000..8a2d4b8 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/DataValidatorGroup.hpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file DataValidatorGroup.hpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include "DataValidator.hpp" + +class DataValidatorGroup +{ +public: + /** + * @param siblings initial number of DataValidator's. Must be > 0. + */ + DataValidatorGroup(unsigned siblings); + ~DataValidatorGroup(); + + /** + * Create a new Validator (with index equal to the number of currently existing validators) + * @return the newly created DataValidator or nullptr on error + */ + DataValidator *add_new_validator(); + + /** + * Put an item into the validator group. + * + * @param index Sensor index + * @param timestamp The timestamp of the measurement + * @param val The 3D vector + * @param error_count The current error count of the sensor + * @param priority The priority of the sensor + */ + void put(unsigned index, uint64_t timestamp, const float val[3], uint32_t error_count, uint8_t priority); + + /** + * Get the best data triplet of the group + * + * @return pointer to the array of best values + */ + float *get_best(uint64_t timestamp, int *index); + + /** + * Get the number of failover events + * + * @return the number of failovers + */ + unsigned failover_count() const { return _toggle_count; } + + /** + * Get the index of the failed sensor in the group + * + * @return index of the failed sensor + */ + int failover_index(); + + /** + * Get the error state of the failed sensor in the group + * + * @return bitmask with erro states of the failed sensor + */ + uint32_t failover_state(); + + /** + * Get the error state of the sensor with the specified index + * + * @return bitmask with error states of the sensor + */ + uint32_t get_sensor_state(unsigned index); + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value for the whole group + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint32_t timeout_interval_us); + + /** + * Set the equal count threshold for the whole group + * + * @param threshold The number of equal values before considering the sensor stale + */ + void set_equal_value_threshold(uint32_t threshold); + +private: + DataValidator *_first{nullptr}; /**< first node in the group */ + DataValidator *_last{nullptr}; /**< last node in the group */ + + uint32_t _timeout_interval_us{0}; /**< currently set timeout */ + + int _curr_best{-1}; /**< currently best index */ + int _prev_best{-1}; /**< the previous best index */ + + uint64_t _first_failover_time{0}; /**< timestamp where the first failover occured or zero if none occured */ + + unsigned _toggle_count{0}; /**< number of back and forth switches between two sensors */ + + static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; + + /* we don't want this class to be copied */ + DataValidatorGroup(const DataValidatorGroup &); + DataValidatorGroup operator=(const DataValidatorGroup &); +}; diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/tests/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/data_validator/tests/CMakeLists.txt new file mode 100644 index 0000000..641b178 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/tests/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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_executable(ecl_tests_data_validator test_data_validator.cpp tests_common.cpp) +target_link_libraries(ecl_tests_data_validator ecl_validation) + +add_test(NAME ecl_tests_data_validator + COMMAND ecl_tests_data_validator + ) + +add_executable(ecl_tests_data_validator_group test_data_validator_group.cpp tests_common.cpp) +target_link_libraries(ecl_tests_data_validator_group ecl_validation) + +add_test(NAME ecl_tests_data_validator_group + COMMAND ecl_tests_data_validator_group + ) diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/tests/test_data_validator.cpp b/src/prometheus_px4/src/modules/sensors/data_validator/tests/test_data_validator.cpp new file mode 100644 index 0000000..56d1238 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/tests/test_data_validator.cpp @@ -0,0 +1,302 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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 test_data_validator.cpp + * Testing the DataValidator class + * + * @author Todd Stellanova + */ + +#include +#include +#include +#include +#include +#include +#include + + +void test_init() +{ + printf("\n--- test_init ---\n"); + + uint64_t fake_timestamp = 666; + const uint32_t timeout_usec = 2000;//from original private value + + DataValidator *validator = new DataValidator; + // initially there should be no siblings + assert(nullptr == validator->sibling()); + // initially we should have zero confidence + assert(0.0f == validator->confidence(fake_timestamp)); + // initially the error count should be zero + assert(0 == validator->error_count()); + // initially unused + assert(!validator->used()); + // initially no priority + assert(0 == validator->priority()); + validator->set_timeout(timeout_usec); + assert(validator->get_timeout() == timeout_usec); + + + DataValidator *sibling_validator = new DataValidator; + validator->setSibling(sibling_validator); + assert(sibling_validator == validator->sibling()); + + //verify that with no data, confidence is zero and error mask is set + assert(0.0f == validator->confidence(fake_timestamp + 1)); + uint32_t state = validator->state(); + assert(DataValidator::ERROR_FLAG_NO_DATA == (DataValidator::ERROR_FLAG_NO_DATA & state)); + + //verify that calling print doesn't crash tests + validator->print(); + + delete validator; //force delete +} + +void test_put() +{ + printf("\n--- test_put ---\n"); + + uint64_t timestamp = 500; + const uint32_t timeout_usec = 2000;//derived from class-private value + float val = 3.14159f; + //derived from class-private value: this is min change needed to avoid stale detection + const float sufficient_incr_value = (1.1f * 1E-6f); + + DataValidator *validator = new DataValidator; + fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp); + + assert(validator->used()); + //verify that the last value we inserted is the current validator value + float last_val = val - sufficient_incr_value; + assert(validator->value()[0] == last_val); + + // we've just provided a bunch of valid data: should be fully confident + float conf = validator->confidence(timestamp); + + if (1.0f != conf) { + printf("conf: %f\n", (double)conf); + dump_validator_state(validator); + } + + assert(1.0f == conf); + // should be no errors + assert(0 == validator->state()); + + //now check confidence much beyond the timeout window-- should timeout + conf = validator->confidence(timestamp + (1.1 * timeout_usec)); + + if (0.0f != conf) { + printf("conf: %f\n", (double)conf); + dump_validator_state(validator); + } + + assert(0.0f == conf); + assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state())); + + delete validator; //force delete +} + +/** + * Verify that the DataValidator detects sensor data that does not vary sufficiently + */ +void test_stale_detector() +{ + printf("\n--- test_stale_detector ---\n"); + + uint64_t timestamp = 500; + float val = 3.14159f; + //derived from class-private value, this is insufficient to avoid stale detection: + const float insufficient_incr_value = (0.99 * 1E-6f); + + DataValidator *validator = new DataValidator; + fill_validator_with_samples(validator, insufficient_incr_value, &val, ×tamp); + + // data is stale: should have no confidence + assert(0.0f == validator->confidence(timestamp)); + + // should be a stale error + uint32_t state = validator->state(); + + if (DataValidator::ERROR_FLAG_STALE_DATA != state) { + dump_validator_state(validator); + } + + assert(DataValidator::ERROR_FLAG_STALE_DATA == (DataValidator::ERROR_FLAG_STALE_DATA & state)); + + delete validator; //force delete +} + +/** + * Verify the RMS error calculated by the DataValidator for a series of samples + */ +void test_rms_calculation() +{ + printf("\n--- test_rms_calculation ---\n"); + const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT + const float mean_value = 3.14159f; + const uint32_t sample_count = 1000; + float expected_rms_err = 0.0f; + uint64_t timestamp = 500; + + DataValidator *validator = new DataValidator; + validator->set_equal_value_threshold(equal_value_count); + + insert_values_around_mean(validator, mean_value, sample_count, &expected_rms_err, ×tamp); + float *rms = validator->rms(); + assert(nullptr != rms); + float calc_rms_err = rms[0]; + float diff = fabsf(calc_rms_err - expected_rms_err); + float diff_frac = (diff / expected_rms_err); + printf("rms: %f expect: %f diff: %f frac: %f\n", (double)calc_rms_err, (double)expected_rms_err, + (double)diff, (double)diff_frac); + assert(diff_frac < 0.03f); + + delete validator; //force delete +} + +/** + * Verify error tracking performed by DataValidator::put + */ +void test_error_tracking() +{ + printf("\n--- test_error_tracking ---\n"); + uint64_t timestamp = 500; + uint64_t timestamp_incr = 5; + const uint32_t timeout_usec = 2000;//from original private value + float val = 3.14159f; + uint32_t error_count = 0; + int expected_error_density = 0; + uint8_t priority = 50; + //from private value: this is min change needed to avoid stale detection + const float sufficient_incr_value = (1.1f * 1E-6f); + //default is private VALUE_EQUAL_COUNT_DEFAULT + const int equal_value_count = 50000; + //should be less than equal_value_count: ensure this is less than NORETURN_ERRCOUNT + const int total_iterations = 1000; + + DataValidator *validator = new DataValidator; + validator->set_timeout(timeout_usec); + validator->set_equal_value_threshold(equal_value_count); + + //put a bunch of values that are all different + for (int i = 0; i < total_iterations; i++, val += sufficient_incr_value) { + timestamp += timestamp_incr; + + //up to a 50% random error rate appears to pass the error density filter + if ((((float)rand() / (float)RAND_MAX)) < 0.500f) { + error_count += 1; + expected_error_density += 1; + + } else if (expected_error_density > 0) { + expected_error_density -= 1; + } + + validator->put(timestamp, val, error_count, priority); + } + + assert(validator->used()); + //at this point, error_count should be less than NORETURN_ERRCOUNT + assert(validator->error_count() == error_count); + + // we've just provided a bunch of valid data with some errors: + // confidence should be reduced by the number of errors + float conf = validator->confidence(timestamp); + printf("error_count: %u validator confidence: %f\n", (uint32_t)error_count, (double)conf); + assert(1.0f != conf); //we should not be fully confident + assert(0.0f != conf); //neither should we be completely unconfident + // should be no errors, even if confidence is reduced, since we didn't exceed NORETURN_ERRCOUNT + assert(0 == validator->state()); + + // the error density will reduce the confidence by 1 - (error_density / ERROR_DENSITY_WINDOW) + // ERROR_DENSITY_WINDOW is currently private, but == 100.0f + float reduced_conf = 1.0f - ((float)expected_error_density / 100.0f); + double diff = fabs(reduced_conf - conf); + + if (reduced_conf != conf) { + printf("conf: %f reduced_conf: %f diff: %f\n", + (double)conf, (double)reduced_conf, diff); + dump_validator_state(validator); + } + + assert(diff < 1E-6f); + + //Now, insert a series of errors and ensure we trip the error detector + for (int i = 0; i < 250; i++, val += sufficient_incr_value) { + timestamp += timestamp_incr; + //100% error rate + error_count += 1; + expected_error_density += 1; + validator->put(timestamp, val, error_count, priority); + } + + conf = validator->confidence(timestamp); + assert(0.0f == conf); // should we be completely unconfident + // we should have triggered the high error density detector + assert(DataValidator::ERROR_FLAG_HIGH_ERRDENSITY == (DataValidator::ERROR_FLAG_HIGH_ERRDENSITY & validator->state())); + + + validator->reset_state(); + + //Now insert so many errors that we exceed private NORETURN_ERRCOUNT + for (int i = 0; i < 10000; i++, val += sufficient_incr_value) { + timestamp += timestamp_incr; + //100% error rate + error_count += 1; + expected_error_density += 1; + validator->put(timestamp, val, error_count, priority); + } + + conf = validator->confidence(timestamp); + assert(0.0f == conf); // should we be completely unconfident + // we should have triggered the high error count detector + assert(DataValidator::ERROR_FLAG_HIGH_ERRCOUNT == (DataValidator::ERROR_FLAG_HIGH_ERRCOUNT & validator->state())); + + delete validator; //force delete + +} + +int main(int argc, char *argv[]) +{ + (void)argc; // unused + (void)argv; // unused + + srand(666); + test_init(); + test_put(); + test_stale_detector(); + test_rms_calculation(); + test_error_tracking(); + + return 0; //passed +} diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/tests/test_data_validator_group.cpp b/src/prometheus_px4/src/modules/sensors/data_validator/tests/test_data_validator_group.cpp new file mode 100644 index 0000000..2af5c2a --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/tests/test_data_validator_group.cpp @@ -0,0 +1,385 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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 test_data_validator_group.cpp + * Testing the DataValidatorGroup class + * + * @author Todd Stellanova + */ + +#include +#include +#include +#include +#include +#include +#include +#include + + +const uint32_t base_timeout_usec = 2000;//from original private value +const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT +const uint64_t base_timestamp = 666; +const unsigned base_num_siblings = 4; + + +/** + * Initialize a DataValidatorGroup with some common settings; + * @param sibling_count (out) the number of siblings initialized + */ +DataValidatorGroup *setup_base_group(unsigned *sibling_count) +{ + unsigned num_siblings = base_num_siblings; + + DataValidatorGroup *group = new DataValidatorGroup(num_siblings); + assert(nullptr != group); + //verify that calling print doesn't crash the tests + group->print(); + printf("\n"); + + //should be no failovers yet + assert(0 == group->failover_count()); + assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state()); + assert(-1 == group->failover_index()); + + //this sets the timeout on all current members of the group, as well as members added later + group->set_timeout(base_timeout_usec); + //the following sets the threshold on all CURRENT members of the group, but not any added later + //TODO This is likely a bug in DataValidatorGroup + group->set_equal_value_threshold(equal_value_count); + + //return values + *sibling_count = num_siblings; + + return group; +} + +/** + * Fill one DataValidator with samples, by index. + * + * @param group + * @param val1_idx Index of the validator to fill with samples + * @param num_samples + */ +void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx, uint32_t num_samples) +{ + uint64_t timestamp = base_timestamp; + uint32_t error_count = 0; + float last_best_val = 0.0f; + + for (uint32_t i = 0; i < num_samples; i++) { + float val = ((float) rand() / (float) RAND_MAX); + float data[DataValidator::dimensions] = {val}; + group->put(val1_idx, timestamp, data, error_count, 100); + last_best_val = val; + } + + int best_idx = 0; + float *best_data = group->get_best(timestamp, &best_idx); + assert(last_best_val == best_data[0]); + assert(best_idx == val1_idx); +} + + + +/** + * Fill two validators in the group with samples, by index. + * Both validators will be filled with the same data, but + * the priority of the first validator will be higher than the second. + * + * @param group + * @param val1_idx index of the first validator to fill + * @param val2_idx index of the second validator to fill + * @param num_samples + */ +void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples) +{ + uint64_t timestamp = base_timestamp; + uint32_t error_count = 0; + float last_best_val = 0.0f; + + for (uint32_t i = 0; i < num_samples; i++) { + float val = ((float) rand() / (float) RAND_MAX); + float data[DataValidator::dimensions] = {val}; + //two sensors with identical values, but different priorities + group->put(val1_idx, timestamp, data, error_count, 100); + group->put(val2_idx, timestamp, data, error_count, 10); + last_best_val = val; + } + + int best_idx = 0; + float *best_data = group->get_best(timestamp, &best_idx); + assert(last_best_val == best_data[0]); + assert(best_idx == val1_idx); + +} + +/** + * Dynamically add a validator to the group after construction + * @param group + * @return + */ +DataValidator *add_validator_to_group(DataValidatorGroup *group) +{ + DataValidator *validator = group->add_new_validator(); + //verify the previously set timeout applies to the new group member + assert(validator->get_timeout() == base_timeout_usec); + //for testing purposes, ensure this newly added member is consistent with the rest of the group + //TODO this is likely a bug in DataValidatorGroup + validator->set_equal_value_threshold(equal_value_count); + + return validator; +} + +/** + * Create a DataValidatorGroup and tack on two additional DataValidators + * + * @param validator1_handle (out) first DataValidator added to the group + * @param validator2_handle (out) second DataValidator added to the group + * @param sibling_count (in/out) in: number of initial siblings to create, out: total + * @return + */ +DataValidatorGroup *setup_group_with_two_validator_handles( + DataValidator **validator1_handle, + DataValidator **validator2_handle, + unsigned *sibling_count) +{ + DataValidatorGroup *group = setup_base_group(sibling_count); + + //now we add validators + *validator1_handle = add_validator_to_group(group); + *validator2_handle = add_validator_to_group(group); + *sibling_count += 2; + + return group; +} + + +void test_init() +{ + unsigned num_siblings = 0; + + DataValidatorGroup *group = setup_base_group(&num_siblings); + + //should not yet be any best value + int best_index = -1; + assert(nullptr == group->get_best(base_timestamp, &best_index)); + + delete group; //force cleanup +} + + +/** + * Happy path test of put method -- ensure the "best" sensor selected is the one with highest priority + */ +void test_put() +{ + unsigned num_siblings = 0; + DataValidator *validator1 = nullptr; + DataValidator *validator2 = nullptr; + + uint64_t timestamp = base_timestamp; + + DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); + printf("num_siblings: %d \n", num_siblings); + unsigned val1_idx = num_siblings - 2; + unsigned val2_idx = num_siblings - 1; + + fill_two_with_valid_data(group, val1_idx, val2_idx, 500); + int best_idx = -1; + float *best_data = group->get_best(timestamp, &best_idx); + assert(nullptr != best_data); + float best_val = best_data[0]; + + float *cur_val1 = validator1->value(); + assert(nullptr != cur_val1); + //printf("cur_val1 %p \n", cur_val1); + assert(best_val == cur_val1[0]); + + float *cur_val2 = validator2->value(); + assert(nullptr != cur_val2); + //printf("cur_val12 %p \n", cur_val2); + assert(best_val == cur_val2[0]); + + delete group; //force cleanup +} + + +/** + * Verify that the DataValidatorGroup will select the sensor with the latest higher priority as "best". + */ +void test_priority_switch() +{ + unsigned num_siblings = 0; + DataValidator *validator1 = nullptr; + DataValidator *validator2 = nullptr; + + uint64_t timestamp = base_timestamp; + + DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); + //printf("num_siblings: %d \n",num_siblings); + int val1_idx = (int)num_siblings - 2; + int val2_idx = (int)num_siblings - 1; + uint32_t error_count = 0; + + fill_two_with_valid_data(group, val1_idx, val2_idx, 100); + + int best_idx = -1; + float *best_data = nullptr; + //now, switch the priorities, which switches "best" but doesn't trigger a failover + float new_best_val = 3.14159f; + float data[DataValidator::dimensions] = {new_best_val}; + //a single sample insertion should be sufficient to trigger a priority switch + group->put(val1_idx, timestamp, data, error_count, 1); + group->put(val2_idx, timestamp, data, error_count, 100); + best_data = group->get_best(timestamp, &best_idx); + assert(new_best_val == best_data[0]); + //the new best sensor should now be the sensor with the higher priority + assert(best_idx == val2_idx); + //should not have detected a real failover + assert(0 == group->failover_count()); + + delete group; //cleanup +} + +/** + * Verify that the DataGroupValidator will prefer a sensor with no errors over a sensor with high errors + */ +void test_simple_failover() +{ + unsigned num_siblings = 0; + DataValidator *validator1 = nullptr; + DataValidator *validator2 = nullptr; + + uint64_t timestamp = base_timestamp; + + DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); + //printf("num_siblings: %d \n",num_siblings); + int val1_idx = (int)num_siblings - 2; + int val2_idx = (int)num_siblings - 1; + + + fill_two_with_valid_data(group, val1_idx, val2_idx, 100); + + int best_idx = -1; + float *best_data = nullptr; + + //trigger a real failover + float new_best_val = 3.14159f; + float data[DataValidator::dimensions] = {new_best_val}; + //trigger a bunch of errors on the previous best sensor + unsigned val1_err_count = 0; + + for (int i = 0; i < 25; i++) { + group->put(val1_idx, timestamp, data, ++val1_err_count, 100); + group->put(val2_idx, timestamp, data, 0, 10); + } + + assert(validator1->error_count() == val1_err_count); + + //since validator1 is experiencing errors, we should see a failover to validator2 + best_data = group->get_best(timestamp + 1, &best_idx); + assert(nullptr != best_data); + assert(new_best_val == best_data[0]); + assert(best_idx == val2_idx); + //should have detected a real failover + printf("failover_count: %d \n", group->failover_count()); + assert(1 == group->failover_count()); + + //even though validator1 has encountered a bunch of errors, it hasn't failed + assert(DataValidator::ERROR_FLAG_NO_ERROR == validator1->state()); + + // although we failed over from one sensor to another, this is not the same thing tracked by failover_index + int fail_idx = group->failover_index(); + assert(-1 == fail_idx);//no failed sensor + + //since no sensor has actually hard-failed, the group failover state is NO_ERROR + assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state()); + + + delete group; //cleanup +} + +/** + * Force once sensor to fail and ensure that we detect it + */ +void test_sensor_failure() +{ + unsigned num_siblings = 0; + uint64_t timestamp = base_timestamp; + const float sufficient_incr_value = (1.1f * 1E-6f); + const uint32_t timeout_usec = 2000;//derived from class-private value + + float val = 3.14159f; + + DataValidatorGroup *group = setup_base_group(&num_siblings); + + //now we add validators + DataValidator *validator = add_validator_to_group(group); + assert(nullptr != validator); + num_siblings++; + int val_idx = num_siblings - 1; + + fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp); + //the best should now be the one validator we've filled with samples + + int best_idx = -1; + float *best_data = group->get_best(timestamp, &best_idx); + assert(nullptr != best_data); + //printf("best_idx: %d val_idx: %d\n", best_idx, val_idx); + assert(best_idx == val_idx); + + //now force a timeout failure in the one validator, by checking confidence long past timeout + validator->confidence(timestamp + (1.1 * timeout_usec)); + assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state())); + + //now that the one sensor has failed, the group should detect this as well + int fail_idx = group->failover_index(); + assert(val_idx == fail_idx); + + delete group; +} + +int main(int argc, char *argv[]) +{ + (void)argc; // unused + (void)argv; // unused + + test_init(); + test_put(); + test_simple_failover(); + test_priority_switch(); + test_sensor_failure(); + + return 0; //passed +} diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/tests/tests_common.cpp b/src/prometheus_px4/src/modules/sensors/data_validator/tests/tests_common.cpp new file mode 100644 index 0000000..899f787 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/tests/tests_common.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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 +#include "tests_common.h" + + +void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, + uint64_t *timestamp_io) +{ + uint64_t timestamp = *timestamp_io; + uint64_t timestamp_incr = 5; + const uint32_t error_count = 0; + const uint8_t priority = 50; + const float swing = 1E-2f; + double sum_dev_squares = 0.0f; + + //insert a series of values that swing around the mean + for (uint32_t i = 0; i < count; i++) { + float iter_swing = (0 == (i % 2)) ? swing : -swing; + float iter_val = mean + iter_swing; + float iter_dev = iter_val - mean; + sum_dev_squares += (iter_dev * iter_dev); + timestamp += timestamp_incr; + validator->put(timestamp, iter_val, error_count, priority); + } + + double rms = sqrt(sum_dev_squares / (double)count); + //note: this should be approximately equal to "swing" + *rms_err = (float)rms; + *timestamp_io = timestamp; +} + +void dump_validator_state(DataValidator *validator) +{ + uint32_t state = validator->state(); + printf("state: 0x%x no_data: %d stale: %d timeout:%d\n", + validator->state(), + DataValidator::ERROR_FLAG_NO_DATA & state, + DataValidator::ERROR_FLAG_STALE_DATA & state, + DataValidator::ERROR_FLAG_TIMEOUT & state + ); + validator->print(); + printf("\n"); +} + +void fill_validator_with_samples(DataValidator *validator, + const float incr_value, + float *value_io, + uint64_t *timestamp_io) +{ + uint64_t timestamp = *timestamp_io; + const uint64_t timestamp_incr = 5; //usec + const uint32_t timeout_usec = 2000;//derived from class-private value + + float val = *value_io; + const uint32_t error_count = 0; + const uint8_t priority = 50; //"medium" priority + const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT + + validator->set_equal_value_threshold(equal_value_count); + validator->set_timeout(timeout_usec); + + //put a bunch of values that are all different + for (int i = 0; i < equal_value_count; i++, val += incr_value) { + timestamp += timestamp_incr; + validator->put(timestamp, val, error_count, priority); + } + + *timestamp_io = timestamp; + *value_io = val; + +} diff --git a/src/prometheus_px4/src/modules/sensors/data_validator/tests/tests_common.h b/src/prometheus_px4/src/modules/sensors/data_validator/tests/tests_common.h new file mode 100644 index 0000000..ae1351d --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/data_validator/tests/tests_common.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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. + * + ****************************************************************************/ + +#ifndef ECL_TESTS_COMMON_H +#define ECL_TESTS_COMMON_H + +#include + +/** + * Insert a series of samples around a mean value + * @param validator The validator under test + * @param mean The mean value + * @param count The number of samples to insert in the validator + * @param rms_err (out) calculated rms error of the inserted samples + * @param timestamp_io (in/out) in: start timestamp, out: last timestamp + */ +void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, + uint64_t *timestamp_io); + +/** + * Print out the state of a DataValidator + * @param validator + */ +void dump_validator_state(DataValidator *validator); + +/** + * Insert a time series of samples into the validator + * @param validator + * @param incr_value The amount to increment the value by on each iteration + * @param value_io (in/out) in: initial value, out: final value + * @param timestamp_io (in/out) in: initial timestamp, out: final timestamp + */ +void fill_validator_with_samples(DataValidator *validator, + const float incr_value, + float *value_io, + uint64_t *timestamp_io); + +#endif //ECL_TESTS_COMMON_H diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params.c b/src/prometheus_px4/src/modules/sensors/sensor_params.c new file mode 100644 index 0000000..60778f7 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params.c @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Airspeed sensor compensation model for the SDP3x + * + * Model with Pitot + * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. + * Model without Pitot (1.5 mm tubes) + * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. + * Tube Pressure Drop + * CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot. + * + * @value 0 Model with Pitot + * @value 1 Model without Pitot (1.5 mm tubes) + * @value 2 Tube Pressure Drop + * + * @group Sensors + */ +PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0); + +/** + * Airspeed sensor tube length. + * + * See the CAL_AIR_CMODEL explanation on how this parameter should be set. + * + * @min 0.01 + * @max 2.00 + * @unit m + * + * @group Sensors + */ +PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f); + +/** + * Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation. + * + * @min 0.1 + * @max 100 + * @unit mm + * + * @group Sensors + */ +PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f); + +/** + * Differential pressure sensor offset + * + * The offset (zero-reading) in Pascal + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); + +/** + * Differential pressure sensor analog scaling + * + * Pick the appropriate scaling from the datasheet. + * this number defines the (linear) conversion from voltage + * to Pascal (pa). For the MPXV7002DP this is 1000. + * + * NOTE: If the sensor always registers zero, try switching + * the static and dynamic tubes. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); + +/** + * Board rotation + * + * This parameter defines the rotation of the FMU board relative to the platform. + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); + +/** + * Board rotation Y (Pitch) offset + * + * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @unit deg + * @group Sensors + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); + +/** + * Board rotation X (Roll) offset + * + * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @unit deg + * @group Sensors + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); + +/** + * Board rotation Z (YAW) offset + * + * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @unit deg + * @group Sensors + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); + +/** + * Thermal control of sensor temperature + * + * @value -1 Thermal control unavailable + * @value 0 Thermal control off + * @value 1 Thermal control enabled + * @category system + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1); + +/** + * External I2C probe. + * + * Probe for optional external I2C devices. + * + * @boolean + * @category system + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_EXT_I2C_PRB, 1); + +/** + * Sensors hub IMU mode + * + * @value 0 Disabled + * @value 1 Publish primary IMU selection + * + * @category system + * @reboot_required true + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_IMU_MODE, 1); + +/** + * Enable internal barometers + * + * For systems with an external barometer, this should be set to false to make sure that the external is used. + * + * @boolean + * @reboot_required true + * @category system + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_INT_BARO_EN, 1); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_acc0.c b/src/prometheus_px4/src/modules/sensors/sensor_params_acc0.c new file mode 100644 index 0000000..992b8d0 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_acc0.c @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of the Accelerometer that the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC0_ID, 0); + +/** + * Accelerometer 0 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC0_PRIO, -1); + +/** + * Rotation of accelerometer 0 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC0_ROT, -1); + +/** + * Accelerometer X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_ZOFF, 0.0f); + +/** + * Accelerometer X-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_acc1.c b/src/prometheus_px4/src/modules/sensors/sensor_params_acc1.c new file mode 100644 index 0000000..c8d17f4 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_acc1.c @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of the Accelerometer that the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC1_ID, 0); + +/** + * Accelerometer 1 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC1_PRIO, -1); + +/** + * Rotation of accelerometer 1 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC1_ROT, -1); + +/** + * Accelerometer X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC1_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC1_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC1_ZOFF, 0.0f); + +/** + * Accelerometer X-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC1_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC1_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_acc2.c b/src/prometheus_px4/src/modules/sensors/sensor_params_acc2.c new file mode 100644 index 0000000..b97d671 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_acc2.c @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of the Accelerometer that the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC2_ID, 0); + +/** + * Accelerometer 2 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC2_PRIO, -1); + +/** + * Rotation of accelerometer 2 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC2_ROT, -1); + +/** + * Accelerometer X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_ZOFF, 0.0f); + +/** + * Accelerometer X-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_acc3.c b/src/prometheus_px4/src/modules/sensors/sensor_params_acc3.c new file mode 100644 index 0000000..807b624 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_acc3.c @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of the Accelerometer that the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC3_ID, 0); + +/** + * Accelerometer 3 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC3_PRIO, -1); + +/** + * Rotation of accelerometer 3 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC3_ROT, -1); + +/** + * Accelerometer X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC3_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC3_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC3_ZOFF, 0.0f); + +/** + * Accelerometer X-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC3_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC3_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_ACC3_ZSCALE, 1.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_flow.c b/src/prometheus_px4/src/modules/sensors/sensor_params_flow.c new file mode 100644 index 0000000..b8a2b75 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_flow.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * PX4Flow board rotation + * + * This parameter defines the yaw rotation of the PX4FLOW board relative to the vehicle body frame. + * Zero rotation is defined as X on flow board pointing towards front of vehicle. + * The recommneded installation default for the PX4FLOW board is with the Y axis forward (270 deg yaw). + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * + * @reboot_required true + * + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6); + +/** + * Minimum height above ground when reliant on optical flow. + * + * This parameter defines the minimum distance from ground at which the optical flow sensor operates reliably. + * The sensor may be usable below this height, but accuracy will progressively reduce to loss of focus. + * + * @unit m + * @min 0.0 + * @max 1.0 + * @increment 0.1 + * @decimal 1 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f); + +/** + * Maximum height above ground when reliant on optical flow. + * + * This parameter defines the maximum distance from ground at which the optical flow sensor operates reliably. + * The height setpoint will be limited to be no greater than this value when the navigation system + * is completely reliant on optical flow data and the height above ground estimate is valid. + * The sensor may be usable above this height, but accuracy will progressively degrade. + * + * @unit m + * @min 1.0 + * @max 25.0 + * @increment 0.1 + * @decimal 1 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f); + +/** + * Magnitude of maximum angular flow rate reliably measurable by the optical flow sensor. + * + * Optical flow data will not fused by the estimators if the magnitude of the flow rate exceeds this value and + * control loops will be instructed to limit ground speed such that the flow rate produced by movement over ground + * is less than 50% of this value. + * + * @unit rad/s + * @min 1.0 + * @decimal 2 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 2.5f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_gyro0.c b/src/prometheus_px4/src/modules/sensors/sensor_params_gyro0.c new file mode 100644 index 0000000..3b66934 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_gyro0.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of the Gyro that the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0); + +/** + * Gyro 0 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, -1); + +/** + * Rotation of gyro 0 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO0_ROT, -1); + +/** + * Gyro X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_XOFF, 0.0f); + +/** + * Gyro Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0f); + +/** + * Gyro Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_gyro1.c b/src/prometheus_px4/src/modules/sensors/sensor_params_gyro1.c new file mode 100644 index 0000000..4545984 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_gyro1.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of the Gyro that the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0); + +/** + * Gyro 1 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, -1); + +/** + * Rotation of gyro 1 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO1_ROT, -1); + +/** + * Gyro X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO1_XOFF, 0.0f); + +/** + * Gyro Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0f); + +/** + * Gyro Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_gyro2.c b/src/prometheus_px4/src/modules/sensors/sensor_params_gyro2.c new file mode 100644 index 0000000..5f5ae34 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_gyro2.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of the Gyro that the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0); + +/** + * Gyro 2 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, -1); + +/** + * Rotation of gyro 2 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO2_ROT, -1); + +/** + * Gyro X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_XOFF, 0.0f); + +/** + * Gyro Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0f); + +/** + * Gyro Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_gyro3.c b/src/prometheus_px4/src/modules/sensors/sensor_params_gyro3.c new file mode 100644 index 0000000..bfa1210 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_gyro3.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of the Gyro that the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO3_ID, 0); + +/** + * Gyro 3 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO3_PRIO, -1); + +/** + * Rotation of gyro 3 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO3_ROT, -1); + +/** + * Gyro X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO3_XOFF, 0.0f); + +/** + * Gyro Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO3_YOFF, 0.0f); + +/** + * Gyro Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_GYRO3_ZOFF, 0.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_mag.c b/src/prometheus_px4/src/modules/sensors/sensor_params_mag.c new file mode 100644 index 0000000..091e37b --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_mag.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * Bitfield selecting mag sides for calibration + * + * If set to two side calibration, only the offsets are estimated, the scale + * calibration is left unchanged. Thus an initial six side calibration is + * recommended. + * + * Bits: + * ORIENTATION_TAIL_DOWN = 1 + * ORIENTATION_NOSE_DOWN = 2 + * ORIENTATION_LEFT = 4 + * ORIENTATION_RIGHT = 8 + * ORIENTATION_UPSIDE_DOWN = 16 + * ORIENTATION_RIGHTSIDE_UP = 32 + * + * @min 34 + * @max 63 + * @value 34 Two side calibration + * @value 38 Three side calibration + * @value 63 Six side calibration + * @group Sensors + */ +PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63); + +/** + * Type of magnetometer compensation + * + * @value 0 Disabled + * @value 1 Throttle-based compensation + * @value 2 Current-based compensation (battery_status instance 0) + * @value 3 Current-based compensation (battery_status instance 1) + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0); + +/** + * Automatically set external rotations. + * + * During calibration attempt to automatically determine the rotation of external magnetometers. + * + * @boolean + * @group Sensors + */ +PARAM_DEFINE_INT32(CAL_MAG_ROT_AUTO, 1); + +/** + * Magnetometer max rate. + * + * Magnetometer data maximum publication rate. This is an upper bound, + * actual magnetometer data rate is still dependant on the sensor. + * + * @min 1 + * @max 200 + * @group Sensors + * @unit Hz + * + * @reboot_required true + * + */ +PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f); + +/** + * Sensors hub mag mode + * + * @value 0 Publish all magnetometers + * @value 1 Publish primary magnetometer + * + * @category system + * @reboot_required true + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_MAG_MODE, 1); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_mag0.c b/src/prometheus_px4/src/modules/sensors/sensor_params_mag0.c new file mode 100644 index 0000000..170885d --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_mag0.c @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of Magnetometer the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); + +/** + * Mag 0 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG0_PRIO, -1); + +/** + * Rotation of magnetometer 0 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG0_ROT, -1); + +/** + * Magnetometer X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_XOFF, 0.0f); + +/** + * Magnetometer Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_YOFF, 0.0f); + +/** + * Magnetometer Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZOFF, 0.0f); + +/** + * Magnetometer X-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f); + +/** + * Magnetometer X-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_XODIAG, 0.0f); + +/** + * Magnetometer Y-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_YODIAG, 0.0f); + +/** + * Magnetometer Z-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZODIAG, 0.0f); + +/** +* X Axis throttle compensation for Mag 0 +* +* Coefficient describing linear relationship between +* X component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG0_XCOMP, 0.0f); + +/** +* Y Axis throttle compensation for Mag 0 +* +* Coefficient describing linear relationship between +* Y component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG0_YCOMP, 0.0f); + +/** +* Z Axis throttle compensation for Mag 0 +* +* Coefficient describing linear relationship between +* Z component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZCOMP, 0.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_mag1.c b/src/prometheus_px4/src/modules/sensors/sensor_params_mag1.c new file mode 100644 index 0000000..5ad5865 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_mag1.c @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of Magnetometer the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); + +/** + * Mag 1 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG1_PRIO, -1); + +/** + * Rotation of magnetometer 1 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG1_ROT, -1); + +/** + * Magnetometer X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_XOFF, 0.0f); + +/** + * Magnetometer Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_YOFF, 0.0f); + +/** + * Magnetometer Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_ZOFF, 0.0f); + +/** + * Magnetometer X-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f); + +/** + * Magnetometer X-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_XODIAG, 0.0f); + +/** + * Magnetometer Y-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_YODIAG, 0.0f); + +/** + * Magnetometer Z-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_ZODIAG, 0.0f); + +/** +* X Axis throttle compensation for Mag 1 +* +* Coefficient describing linear relationship between +* X component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG1_XCOMP, 0.0f); + +/** +* Y Axis throttle compensation for Mag 1 +* +* Coefficient describing linear relationship between +* Y component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG1_YCOMP, 0.0f); + +/** +* Z Axis throttle compensation for Mag 1 +* +* Coefficient describing linear relationship between +* Z component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG1_ZCOMP, 0.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_mag2.c b/src/prometheus_px4/src/modules/sensors/sensor_params_mag2.c new file mode 100644 index 0000000..d2f7b5c --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_mag2.c @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of Magnetometer the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); + +/** + * Mag 2 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG2_PRIO, -1); + +/** + * Rotation of magnetometer 2 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG2_ROT, -1); + +/** + * Magnetometer X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_XOFF, 0.0f); + +/** + * Magnetometer Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_YOFF, 0.0f); + +/** + * Magnetometer Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZOFF, 0.0f); + +/** + * Magnetometer X-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f); + +/** + * Magnetometer X-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_XODIAG, 0.0f); + +/** + * Magnetometer Y-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_YODIAG, 0.0f); + +/** + * Magnetometer Z-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZODIAG, 0.0f); + +/** +* X Axis throttle compensation for Mag 2 +* +* Coefficient describing linear relationship between +* X component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG2_XCOMP, 0.0f); + +/** +* Y Axis throttle compensation for Mag 2 +* +* Coefficient describing linear relationship between +* Y component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG2_YCOMP, 0.0f); + +/** +* Z Axis throttle compensation for Mag 2 +* +* Coefficient describing linear relationship between +* Z component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZCOMP, 0.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensor_params_mag3.c b/src/prometheus_px4/src/modules/sensors/sensor_params_mag3.c new file mode 100644 index 0000000..56f070f --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensor_params_mag3.c @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * ID of Magnetometer the calibration is for. + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG3_ID, 0); + +/** + * Mag 3 priority. + * + * @value -1 Uninitialized + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG3_PRIO, -1); + +/** + * Rotation of magnetometer 3 relative to airframe. + * + * An internal sensor will force a value of -1, so a GCS + * should only attempt to configure the rotation if the value is + * greater than or equal to zero. + * + * @value -1 Internal + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + * @value 8 Roll 180° + * @value 9 Roll 180°, Yaw 45° + * @value 10 Roll 180°, Yaw 90° + * @value 11 Roll 180°, Yaw 135° + * @value 12 Pitch 180° + * @value 13 Roll 180°, Yaw 225° + * @value 14 Roll 180°, Yaw 270° + * @value 15 Roll 180°, Yaw 315° + * @value 16 Roll 90° + * @value 17 Roll 90°, Yaw 45° + * @value 18 Roll 90°, Yaw 90° + * @value 19 Roll 90°, Yaw 135° + * @value 20 Roll 270° + * @value 21 Roll 270°, Yaw 45° + * @value 22 Roll 270°, Yaw 90° + * @value 23 Roll 270°, Yaw 135° + * @value 24 Pitch 90° + * @value 25 Pitch 270° + * @value 26 Pitch 180°, Yaw 90° + * @value 27 Pitch 180°, Yaw 270° + * @value 28 Roll 90°, Pitch 90° + * @value 29 Roll 180°, Pitch 90° + * @value 30 Roll 270°, Pitch 90° + * @value 31 Roll 90°, Pitch 180° + * @value 32 Roll 270°, Pitch 180° + * @value 33 Roll 90°, Pitch 270° + * @value 34 Roll 180°, Pitch 270° + * @value 35 Roll 270°, Pitch 270° + * @value 36 Roll 90°, Pitch 180°, Yaw 90° + * @value 37 Roll 90°, Yaw 270° + * @value 38 Roll 90°, Pitch 68°, Yaw 293° + * @value 39 Pitch 315° + * @value 40 Roll 90°, Pitch 315° + * + * @min -1 + * @max 40 + * @reboot_required true + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG3_ROT, -1); + +/** + * Magnetometer X-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_XOFF, 0.0f); + +/** + * Magnetometer Y-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_YOFF, 0.0f); + +/** + * Magnetometer Z-axis offset + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_ZOFF, 0.0f); + +/** + * Magnetometer X-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0f); + +/** + * Magnetometer X-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_XODIAG, 0.0f); + +/** + * Magnetometer Y-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_YODIAG, 0.0f); + +/** + * Magnetometer Z-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + * @volatile + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_ZODIAG, 0.0f); + +/** +* X Axis throttle compensation for Mag 3 +* +* Coefficient describing linear relationship between +* X component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG3_XCOMP, 0.0f); + +/** +* Y Axis throttle compensation for Mag 3 +* +* Coefficient describing linear relationship between +* Y component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG3_YCOMP, 0.0f); + +/** +* Z Axis throttle compensation for Mag 3 +* +* Coefficient describing linear relationship between +* Z component of magnetometer in body frame axis +* and either current or throttle depending on value of CAL_MAG_COMP_TYP. +* Unit for throttle-based compensation is [G] and +* for current-based compensation [G/kA] +* +* @category system +* @group Sensor Calibration +*/ +PARAM_DEFINE_FLOAT(CAL_MAG3_ZCOMP, 0.0f); diff --git a/src/prometheus_px4/src/modules/sensors/sensors.cpp b/src/prometheus_px4/src/modules/sensors/sensors.cpp new file mode 100644 index 0000000..9e3a5c4 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/sensors.cpp @@ -0,0 +1,836 @@ +/**************************************************************************** + * + * 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 sensors.cpp + * + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler + * @author Anton Babushkin + * @author Beat Küng + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "voted_sensors_update.h" +#include "vehicle_acceleration/VehicleAcceleration.hpp" +#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" +#include "vehicle_air_data/VehicleAirData.hpp" +#include "vehicle_gps_position/VehicleGPSPosition.hpp" +#include "vehicle_imu/VehicleIMU.hpp" +#include "vehicle_magnetometer/VehicleMagnetometer.hpp" + +using namespace sensors; +using namespace time_literals; + +/** + * HACK - true temperature is much less than indicated temperature in baro, + * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB + */ +#define PCB_TEMP_ESTIMATE_DEG 5.0f +class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + explicit Sensors(bool hil_enabled); + ~Sensors() 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); + + /** @see ModuleBase::run() */ + void Run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + + bool init(); + +private: + const bool _hil_enabled; /**< if true, HIL is active */ + bool _armed{false}; /**< arming status of the vehicle */ + + hrt_abstime _last_config_update{0}; + hrt_abstime _sensor_combined_prev_timestamp{0}; + + sensor_combined_s _sensor_combined{}; + + uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] { + {this, ORB_ID(vehicle_imu), 0}, + {this, ORB_ID(vehicle_imu), 1}, + {this, ORB_ID(vehicle_imu), 2}, + {this, ORB_ID(vehicle_imu), 3} + }; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ + +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + + hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */ + + uORB::Subscription _adc_report_sub{ORB_ID(adc_report)}; /**< adc_report sub */ + differential_pressure_s _diff_pres {}; + uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */ +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + + struct Parameters { + float diff_pres_offset_pa; +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + float diff_pres_analog_scale; +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + int32_t air_cmodel; + float air_tube_length; + float air_tube_diameter_mm; + } _parameters{}; /**< local copies of interesting parameters */ + + struct ParameterHandles { + param_t diff_pres_offset_pa; +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + param_t diff_pres_analog_scale; +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + param_t air_cmodel; + param_t air_tube_length; + param_t air_tube_diameter_mm; + } _parameter_handles{}; /**< handles for interesting parameters */ + + VotedSensorsUpdate _voted_sensors_update; + + VehicleAcceleration _vehicle_acceleration; + VehicleAngularVelocity _vehicle_angular_velocity; + VehicleAirData *_vehicle_air_data{nullptr}; + VehicleMagnetometer *_vehicle_magnetometer{nullptr}; + VehicleGPSPosition *_vehicle_gps_position{nullptr}; + + VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; + + uint8_t _n_accel{0}; + uint8_t _n_baro{0}; + uint8_t _n_gps{0}; + uint8_t _n_gyro{0}; + uint8_t _n_mag{0}; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Poll the differential pressure sensor for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void diff_pres_poll(); + + /** + * 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(); + + void InitializeVehicleAirData(); + void InitializeVehicleGPSPosition(); + void InitializeVehicleIMU(); + void InitializeVehicleMagnetometer(); + + DEFINE_PARAMETERS( + (ParamBool) _param_sys_has_baro, + (ParamBool) _param_sys_has_gps, + (ParamBool) _param_sys_has_mag, + (ParamBool) _param_sens_imu_mode + ) +}; + +Sensors::Sensors(bool hil_enabled) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _hil_enabled(hil_enabled), + _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), + _voted_sensors_update(hil_enabled, _vehicle_imu_sub) +{ + /* Differential pressure offset */ + _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + _parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); + _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); + _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); + + param_find("SYS_FAC_CAL_MODE"); + + // Parameters controlling the on-board sensor thermal calibrator + param_find("SYS_CAL_TDEL"); + param_find("SYS_CAL_TMAX"); + param_find("SYS_CAL_TMIN"); + + _airspeed_validator.set_timeout(300000); + _airspeed_validator.set_equal_value_threshold(100); + + _vehicle_acceleration.Start(); + _vehicle_angular_velocity.Start(); +} + +Sensors::~Sensors() +{ + // clear all registered callbacks + for (auto &sub : _vehicle_imu_sub) { + sub.unregisterCallback(); + } + + _vehicle_acceleration.Stop(); + _vehicle_angular_velocity.Stop(); + + if (_vehicle_air_data) { + _vehicle_air_data->Stop(); + delete _vehicle_air_data; + } + + if (_vehicle_gps_position) { + _vehicle_gps_position->Stop(); + delete _vehicle_gps_position; + } + + if (_vehicle_magnetometer) { + _vehicle_magnetometer->Stop(); + delete _vehicle_magnetometer; + } + + for (auto &vehicle_imu : _vehicle_imu_list) { + if (vehicle_imu) { + vehicle_imu->Stop(); + delete vehicle_imu; + } + } + + perf_free(_loop_perf); +} + +bool Sensors::init() +{ + _vehicle_imu_sub[0].registerCallback(); + ScheduleNow(); + return true; +} + +int Sensors::parameters_update() +{ + if (_armed) { + return 0; + } + + /* Airspeed offset */ + param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel); + param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length); + param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); + + _voted_sensors_update.parametersUpdate(); + + // mark all existing sensor calibrations active even if sensor is missing + // this preserves the calibration in the event of a parameter export while the sensor is missing + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + uint32_t device_id_accel = calibration::GetCalibrationParam("ACC", "ID", i); + uint32_t device_id_gyro = calibration::GetCalibrationParam("GYRO", "ID", i); + uint32_t device_id_mag = calibration::GetCalibrationParam("MAG", "ID", i); + + if (device_id_accel != 0) { + bool external_accel = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0); + calibration::Accelerometer accel_cal(device_id_accel, external_accel); + } + + if (device_id_gyro != 0) { + bool external_gyro = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0); + calibration::Gyroscope gyro_cal(device_id_gyro, external_gyro); + } + + if (device_id_mag != 0) { + bool external_mag = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0); + calibration::Magnetometer mag_cal(device_id_mag, external_mag); + } + } + + // ensure calibration slots are active for the number of sensors currently available + // this to done to eliminate differences in the active set of parameters before and after sensor calibration + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (orb_exists(ORB_ID(sensor_accel), i) == PX4_OK) { + bool external = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0); + calibration::Accelerometer cal{0, external}; + cal.set_calibration_index(i); + cal.ParametersUpdate(); + } + + if (orb_exists(ORB_ID(sensor_gyro), i) == PX4_OK) { + bool external = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0); + calibration::Gyroscope cal{0, external}; + cal.set_calibration_index(i); + cal.ParametersUpdate(); + } + + if (orb_exists(ORB_ID(sensor_mag), i) == PX4_OK) { + bool external = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0); + calibration::Magnetometer cal{0, external}; + cal.set_calibration_index(i); + cal.ParametersUpdate(); + } + } + + return PX4_OK; +} + +void Sensors::diff_pres_poll() +{ + differential_pressure_s diff_pres{}; + + if (_diff_pres_sub.update(&diff_pres)) { + + vehicle_air_data_s air_data{}; + _vehicle_air_data_sub.copy(&air_data); + + float air_temperature_celsius = NAN; + + // assume anything outside of a (generous) operating range of -40C to 125C is invalid + if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { + + air_temperature_celsius = diff_pres.temperature; + + } else { + // differential pressure temperature invalid, check barometer + if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) + && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { + + // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro + air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; + } + } + + airspeed_s airspeed{}; + airspeed.timestamp = diff_pres.timestamp; + + /* push data into validator */ + float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f }; + + _airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? + + airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); + + enum AIRSPEED_SENSOR_MODEL smodel; + + switch ((diff_pres.device_id >> 16) & 0xFF) { + case DRV_DIFF_PRESS_DEVTYPE_SDP31: + + /* fallthrough */ + case DRV_DIFF_PRESS_DEVTYPE_SDP32: + + /* fallthrough */ + case DRV_DIFF_PRESS_DEVTYPE_SDP33: + /* fallthrough */ + smodel = AIRSPEED_SENSOR_MODEL_SDP3X; + break; + + default: + smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE; + break; + } + + /* don't risk to feed negative airspeed into the system */ + airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) + _parameters.air_cmodel, + smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, + diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa, + air_temperature_celsius); + + airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa, + air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here + + airspeed.air_temperature_celsius = air_temperature_celsius; + + if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) { + _airspeed_pub.publish(airspeed); + } + } +} + +void +Sensors::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 + parameters_update(); + updateParams(); + + /* update airspeed scale */ + int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); + + /* this sensor is optional, abort without error */ + if (fd >= 0) { + struct airspeed_scale airscale = { + _parameters.diff_pres_offset_pa, + 1.0f, + }; + + if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + } + + px4_close(fd); + } + } +} + +void Sensors::adc_poll() +{ + /* only read if not in HIL mode */ + if (_hil_enabled) { + return; + } + +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + + if (_parameters.diff_pres_analog_scale > 0.0f) { + + hrt_abstime t = hrt_absolute_time(); + + /* rate limit to 100 Hz */ + if (t - _last_adc >= 10000) { + adc_report_s adc; + + if (_adc_report_sub.update(&adc)) { + /* Read add channels we got */ + for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) { + if (adc.channel_id[i] == -1) { + continue; // skip non-exist channels + } + + if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) { + + /* calculate airspeed, raw is the difference from */ + const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV; + + /** + * The voltage divider pulls the signal down, only act on + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. + * + * Notice: This won't work on devices which have PGA controlled + * vref. Those devices require no divider at all. + */ + if (voltage > 0.4f) { + const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + + _diff_pres.timestamp = t; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + + (diff_pres_pa_raw * 0.1f); + _diff_pres.temperature = -1000.0f; + + _diff_pres_pub.publish(_diff_pres); + } + } + } + } + + _last_adc = t; + } + } + +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ +} + +void Sensors::InitializeVehicleAirData() +{ + if (_param_sys_has_baro.get()) { + if (_vehicle_air_data == nullptr) { + _vehicle_air_data = new VehicleAirData(); + + if (_vehicle_air_data) { + _vehicle_air_data->Start(); + } + } + } +} + +void Sensors::InitializeVehicleGPSPosition() +{ + if (_param_sys_has_gps.get()) { + if (_vehicle_gps_position == nullptr) { + _vehicle_gps_position = new VehicleGPSPosition(); + + if (_vehicle_gps_position) { + _vehicle_gps_position->Start(); + } + } + } +} + +void Sensors::InitializeVehicleIMU() +{ + // create a VehicleIMU instance for each accel/gyro pair + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + if (_vehicle_imu_list[i] == nullptr) { + + uORB::Subscription accel_sub{ORB_ID(sensor_accel), i}; + sensor_accel_s accel{}; + accel_sub.copy(&accel); + + uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i}; + sensor_gyro_s gyro{}; + gyro_sub.copy(&gyro); + + if (accel.device_id > 0 && gyro.device_id > 0) { + // if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ + // otherwise each VehicleIMU runs in a corresponding INSx WQ + const bool multi_mode = (_param_sens_imu_mode.get() == 0); + const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0; + + VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config); + + if (imu != nullptr) { + // Start VehicleIMU instance and store + if (imu->Start()) { + _vehicle_imu_list[i] = imu; + + } else { + delete imu; + } + } + + } else { + // abort on first failure, try again later + return; + } + } + } +} + +void Sensors::InitializeVehicleMagnetometer() +{ + if (_param_sys_has_mag.get()) { + if (_vehicle_magnetometer == nullptr) { + _vehicle_magnetometer = new VehicleMagnetometer(); + + if (_vehicle_magnetometer) { + _vehicle_magnetometer->Start(); + } + } + } +} + +void Sensors::Run() +{ + if (should_exit()) { + // clear all registered callbacks + for (auto &sub : _vehicle_imu_sub) { + sub.unregisterCallback(); + } + + exit_and_cleanup(); + return; + } + + // run once + if (_last_config_update == 0) { + InitializeVehicleAirData(); + InitializeVehicleIMU(); + InitializeVehicleGPSPosition(); + InitializeVehicleMagnetometer(); + _voted_sensors_update.init(_sensor_combined); + parameter_update_poll(true); + } + + perf_begin(_loop_perf); + + // backup schedule as a watchdog timeout + ScheduleDelayed(10_ms); + + // check vehicle status for changes to publication state + if (_vcontrol_mode_sub.updated()) { + vehicle_control_mode_s vcontrol_mode{}; + + if (_vcontrol_mode_sub.copy(&vcontrol_mode)) { + _armed = vcontrol_mode.flag_armed; + } + } + + _voted_sensors_update.sensorsPoll(_sensor_combined); + + // check analog airspeed + adc_poll(); + + diff_pres_poll(); + + if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { + + _voted_sensors_update.setRelativeTimestamps(_sensor_combined); + _sensor_pub.publish(_sensor_combined); + _sensor_combined_prev_timestamp = _sensor_combined.timestamp; + } + + // keep adding sensors as long as we are not armed, + // when not adding sensors poll for param updates + if (!_armed && hrt_elapsed_time(&_last_config_update) > 1000_ms) { + + const int n_accel = orb_group_count(ORB_ID(sensor_accel)); + const int n_baro = orb_group_count(ORB_ID(sensor_baro)); + const int n_gps = orb_group_count(ORB_ID(sensor_gps)); + const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); + const int n_mag = orb_group_count(ORB_ID(sensor_mag)); + + if ((n_accel != _n_accel) || (n_baro != _n_baro) || (n_gps != _n_gps) || (n_gyro != _n_gyro) || (n_mag != _n_mag)) { + _n_accel = n_accel; + _n_baro = n_baro; + _n_gps = n_gps; + _n_gyro = n_gyro; + _n_mag = n_mag; + + parameters_update(); + + InitializeVehicleAirData(); + InitializeVehicleGPSPosition(); + InitializeVehicleMagnetometer(); + } + + // sensor device id (not just orb_group_count) must be populated before IMU init can succeed + _voted_sensors_update.initializeSensors(); + InitializeVehicleIMU(); + + _last_config_update = hrt_absolute_time(); + + } else { + // check parameters for updates + parameter_update_poll(); + } + + perf_end(_loop_perf); +} + +int Sensors::task_spawn(int argc, char *argv[]) +{ + bool hil_enabled = false; + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'h': + hil_enabled = true; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return PX4_ERROR; + } + + Sensors *instance = new Sensors(hil_enabled); + + 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 Sensors::print_status() +{ + _voted_sensors_update.printStatus(); + + if (_vehicle_magnetometer) { + PX4_INFO_RAW("\n"); + _vehicle_magnetometer->PrintStatus(); + } + + if (_vehicle_air_data) { + PX4_INFO_RAW("\n"); + _vehicle_air_data->PrintStatus(); + } + + PX4_INFO_RAW("\n"); + PX4_INFO("Airspeed status:"); + _airspeed_validator.print(); + + PX4_INFO_RAW("\n"); + _vehicle_acceleration.PrintStatus(); + + PX4_INFO_RAW("\n"); + _vehicle_angular_velocity.PrintStatus(); + + if (_vehicle_gps_position) { + PX4_INFO_RAW("\n"); + _vehicle_gps_position->PrintStatus(); + } + + PX4_INFO_RAW("\n"); + + for (auto &i : _vehicle_imu_list) { + if (i != nullptr) { + PX4_INFO_RAW("\n"); + i->PrintStatus(); + } + } + + return 0; +} + +int Sensors::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int Sensors::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The sensors module is central to the whole system. It takes low-level output from drivers, turns +it into a more usable form, and publishes it for the rest of the system. + +The provided functionality includes: +- Read the output from the sensor drivers (`sensor_gyro`, etc.). + If there are multiple of the same type, do voting and failover handling. + Then apply the board rotation and temperature calibration (if enabled). And finally publish the data; one of the + topics is `sensor_combined`, used by many parts of the system. +- Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or + on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the + sensor drivers must already be running when `sensors` is started. +- Do sensor consistency checks and publish the `sensors_status_imu` topic. + +### Implementation +It runs in its own thread and polls on the currently selected gyro topic. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sensors", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('h', "Start in HIL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sensors_main(int argc, char *argv[]) +{ + return Sensors::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/CMakeLists.txt new file mode 100644 index 0000000..37f3558 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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(vehicle_acceleration + VehicleAcceleration.cpp + VehicleAcceleration.hpp +) + +target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +target_link_libraries(vehicle_acceleration + PRIVATE + mathlib + px4_work_queue + sensor_calibration +) diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp new file mode 100644 index 0000000..02c6c0d --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * 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 "VehicleAcceleration.hpp" + +#include + +#include + +using namespace matrix; + +namespace sensors +{ + +VehicleAcceleration::VehicleAcceleration() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + CheckAndUpdateFilters(); +} + +VehicleAcceleration::~VehicleAcceleration() +{ + Stop(); +} + +bool VehicleAcceleration::Start() +{ + // force initial updates + ParametersUpdate(true); + + // sensor_selection needed to change the active sensor if the primary stops updating + if (!_sensor_selection_sub.registerCallback()) { + PX4_ERR("sensor_selection callback registration failed"); + return false; + } + + if (!SensorSelectionUpdate(true)) { + _sensor_sub.registerCallback(); + } + + return true; +} + +void VehicleAcceleration::Stop() +{ + // clear all registered callbacks + _sensor_sub.unregisterCallback(); + _sensor_selection_sub.unregisterCallback(); + + Deinit(); +} + +void VehicleAcceleration::CheckAndUpdateFilters() +{ + bool sample_rate_changed = false; + + // get sample rate from vehicle_imu_status publication + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; + + const float sample_rate_hz = imu_status.get().accel_rate_hz; + + if ((imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id()) + && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) { + // check if sample rate error is greater than 1% + if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { + PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz); + _filter_sample_rate = sample_rate_hz; + sample_rate_changed = true; + + // determine number of sensor samples that will get closest to the desired rate + if (_param_imu_integ_rate.get() > 0) { + const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); + const float sample_interval_avg = 1e6f / sample_rate_hz; + const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f, + (float)sensor_accel_s::ORB_QUEUE_LENGTH); + + _sensor_sub.set_required_updates(samples); + + } else { + _sensor_sub.set_required_updates(1); + } + + break; + } + } + } + + // update software low pass filters + if (sample_rate_changed || (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.1f)) { + _lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get()); + _lp_filter.reset(_acceleration_prev); + } +} + +void VehicleAcceleration::SensorBiasUpdate(bool force) +{ + // find corresponding estimated sensor bias + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + + if (_estimator_sensor_bias_sub.updated() || force) { + estimator_sensor_bias_s bias; + + if (_estimator_sensor_bias_sub.copy(&bias)) { + if (bias.accel_device_id == _calibration.device_id()) { + _bias = Vector3f{bias.accel_bias}; + + } else { + _bias.zero(); + } + } + } +} + +bool VehicleAcceleration::SensorSelectionUpdate(bool force) +{ + if (_sensor_selection_sub.updated() || (_calibration.device_id() == 0) || force) { + sensor_selection_s sensor_selection{}; + _sensor_selection_sub.copy(&sensor_selection); + + if ((sensor_selection.accel_device_id != 0) && (_calibration.device_id() != sensor_selection.accel_device_id)) { + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; + + const uint32_t device_id = sensor_accel_sub.get().device_id; + + if ((device_id != 0) && (device_id == sensor_selection.accel_device_id)) { + + if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { + PX4_DEBUG("selected sensor changed %" PRIu32 " -> %" PRIu32 "", _calibration.device_id(), device_id); + + // clear bias and corrections + _bias.zero(); + + _calibration.set_device_id(device_id); + + CheckAndUpdateFilters(); + + return true; + } + } + } + + PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", sensor_selection.accel_device_id); + _calibration.set_device_id(0); + } + } + + return false; +} + +void VehicleAcceleration::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + + _calibration.ParametersUpdate(); + + CheckAndUpdateFilters(); + } +} + +void VehicleAcceleration::Run() +{ + // backup schedule + ScheduleDelayed(10_ms); + + // update corrections first to set _selected_sensor + bool selection_updated = SensorSelectionUpdate(); + + _calibration.SensorCorrectionsUpdate(selection_updated); + SensorBiasUpdate(selection_updated); + ParametersUpdate(); + + // require valid sensor sample rate to run + if (!PX4_ISFINITE(_filter_sample_rate)) { + CheckAndUpdateFilters(); + + if (!PX4_ISFINITE(_filter_sample_rate)) { + return; + } + } + + // process all outstanding messages + sensor_accel_s sensor_data; + + while (_sensor_sub.update(&sensor_data)) { + const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z}; + + if (math::isFinite(accel_raw)) { + // Apply calibration and filter + // - calibration offsets, scale factors, and thermal scale (if available) + // - estimated in run bias (if available) + // - biquad low-pass filter + const Vector3f accel_corrected = _calibration.Correct(accel_raw) - _bias; + const Vector3f accel_filtered = _lp_filter.apply(accel_corrected); + + _acceleration_prev = accel_corrected; + + // publish once all new samples are processed + if (!_sensor_sub.updated()) { + // Publish vehicle_acceleration + vehicle_acceleration_s v_acceleration; + v_acceleration.timestamp_sample = sensor_data.timestamp_sample; + accel_filtered.copyTo(v_acceleration.xyz); + v_acceleration.timestamp = hrt_absolute_time(); + _vehicle_acceleration_pub.publish(v_acceleration); + + return; + } + } + } +} + +void VehicleAcceleration::PrintStatus() +{ + PX4_INFO("selected sensor: %" PRIu32 ", rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]", + _calibration.device_id(), (double)_filter_sample_rate, + (double)_bias(0), (double)_bias(1), (double)_bias(2)); + + _calibration.PrintStatus(); +} + +} // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp new file mode 100644 index 0000000..68c0652 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ + +class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + VehicleAcceleration(); + ~VehicleAcceleration() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void CheckAndUpdateFilters(); + void ParametersUpdate(bool force = false); + void SensorBiasUpdate(bool force = false); + bool SensorSelectionUpdate(bool force = false); + + static constexpr int MAX_SENSOR_COUNT = 4; + + uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; + + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; + uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_accel)}; + + calibration::Accelerometer _calibration{}; + + matrix::Vector3f _bias{}; + + matrix::Vector3f _acceleration_prev{}; + + float _filter_sample_rate{NAN}; + + math::LowPassFilter2p _lp_filter{}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_imu_accel_cutoff, + (ParamInt) _param_imu_integ_rate + ) +}; + +} // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c b/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c new file mode 100644 index 0000000..c4fda14 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** +* Low pass filter cutoff frequency for accel +* +* The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer. +* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f); diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_air_data/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/vehicle_air_data/CMakeLists.txt new file mode 100644 index 0000000..5d297e1 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_air_data/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(vehicle_air_data + VehicleAirData.cpp + VehicleAirData.hpp +) +target_link_libraries(vehicle_air_data PRIVATE px4_work_queue) diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/prometheus_px4/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp new file mode 100644 index 0000000..0c6bdcf --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -0,0 +1,331 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "VehicleAirData.hpp" + +#include +#include + +namespace sensors +{ + +using namespace matrix; + +static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; + +VehicleAirData::VehicleAirData() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + _voter.set_timeout(SENSOR_TIMEOUT); +} + +VehicleAirData::~VehicleAirData() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleAirData::Start() +{ + ScheduleNow(); + return true; +} + +void VehicleAirData::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } +} + +void VehicleAirData::SensorCorrectionsUpdate(bool force) +{ + if (_sensor_correction_sub.updated() || force) { + sensor_correction_s corrections; + + if (_sensor_correction_sub.copy(&corrections)) { + for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) { + // find sensor (by device id) in sensor_correction + const uint32_t device_id = _last_data[baro_index].device_id; + + if (device_id != 0) { + for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) { + if (corrections.baro_device_ids[correction_index] == device_id) { + switch (correction_index) { + case 0: + _thermal_offset[baro_index] = corrections.baro_offset_0; + break; + + case 1: + _thermal_offset[baro_index] = corrections.baro_offset_1; + break; + + case 2: + _thermal_offset[baro_index] = corrections.baro_offset_2; + break; + + case 3: + _thermal_offset[baro_index] = corrections.baro_offset_3; + break; + } + } + } + } + } + } + } +} + +void VehicleAirData::AirTemperatureUpdate() +{ + differential_pressure_s differential_pressure; + + static constexpr float temperature_min_celsius = -20.f; + static constexpr float temperature_max_celsius = 35.f; + + // update air temperature if data from differential pressure sensor is finite and not exactly 0 + // limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight + if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature) + && fabsf(differential_pressure.temperature) > FLT_EPSILON) { + _air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius, + temperature_max_celsius); + } +} + +void VehicleAirData::ParametersUpdate() +{ + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } +} + +void VehicleAirData::Run() +{ + perf_begin(_cycle_perf); + + ParametersUpdate(); + + SensorCorrectionsUpdate(); + + AirTemperatureUpdate(); + + bool updated[MAX_SENSOR_COUNT] {}; + + for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { + + if (!_advertised[uorb_index]) { + // use data's timestamp to throttle advertisement checks + if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) { + if (_sensor_sub[uorb_index].advertised()) { + if (uorb_index > 0) { + /* the first always exists, but for each further sensor, add a new validator */ + if (!_voter.add_new_validator()) { + PX4_ERR("failed to add validator for %s %i", "BARO", uorb_index); + } + } + + _advertised[uorb_index] = true; + + // force temperature correction update + SensorCorrectionsUpdate(true); + + if (_selected_sensor_sub_index < 0) { + _sensor_sub[uorb_index].registerCallback(); + } + + } else { + _last_data[uorb_index].timestamp = hrt_absolute_time(); + } + } + } + + if (_advertised[uorb_index]) { + updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]); + + if (updated[uorb_index]) { + // millibar to Pa + const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f; + + // pressure corrected with offset (if available) + const float pressure_corrected = (raw_pressure_pascals - _thermal_offset[uorb_index]); + + float vect[3] {pressure_corrected, _last_data[uorb_index].temperature, 0.f}; + _voter.put(uorb_index, _last_data[uorb_index].timestamp, vect, _last_data[uorb_index].error_count, + _priority[uorb_index]); + } + } + } + + // check for the current best sensor + int best_index = 0; + _voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + if (_selected_sensor_sub_index != best_index) { + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } + + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("%s switch from #%" PRIu8 " -> #%d", "BARO", _selected_sensor_sub_index, best_index); + } + + _selected_sensor_sub_index = best_index; + _sensor_sub[_selected_sensor_sub_index].registerCallback(); + } + } + + if ((_selected_sensor_sub_index >= 0) + && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR) + && updated[_selected_sensor_sub_index]) { + + const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index]; + + _baro_timestamp_sum += baro.timestamp_sample; + _baro_sum += baro.pressure; + _baro_sum_count++; + + if ((_param_sens_baro_rate.get() > 0) + && ((_last_publication_timestamp == 0) + || (hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())))) { + + const float pressure = _baro_sum / _baro_sum_count; + const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count; + + // reset + _baro_timestamp_sum = 0; + _baro_sum = 0.f; + _baro_sum_count = 0; + + // populate vehicle_air_data with primary baro and publish + vehicle_air_data_s out{}; + out.timestamp_sample = timestamp_sample; + out.baro_device_id = baro.device_id; + out.baro_temp_celcius = baro.temperature; + + // Convert from millibar to Pa and apply temperature compensation + out.baro_pressure_pa = 100.0f * pressure - _thermal_offset[_selected_sensor_sub_index]; + + // calculate altitude using the hypsometric equation + static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin + static constexpr float a = -6.5f / 1000.0f; // temperature gradient in degrees per metre + + // current pressure at MSL in kPa (QNH in hPa) + const float p1 = _param_sens_baro_qnh.get() * 0.1f; + + // measured pressure in kPa + const float p = out.baro_pressure_pa * 0.001f; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + out.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a; + + // calculate air density + out.rho = out.baro_pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius - + CONSTANTS_ABSOLUTE_NULL_CELSIUS)); + + out.timestamp = hrt_absolute_time(); + _vehicle_air_data_pub.publish(out); + + _last_publication_timestamp = out.timestamp; + } + } + + // check failover and report + if (_last_failover_count != _voter.failover_count()) { + uint32_t flags = _voter.failover_state(); + int failover_index = _voter.failover_index(); + + if (flags != DataValidator::ERROR_FLAG_NO_ERROR) { + if (failover_index != -1) { + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_error_message > 3_s) { + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!", + "BARO", + failover_index, + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + _last_error_message = now; + } + + // reduce priority of failed sensor to the minimum + _priority[failover_index] = 1; + } + } + + _last_failover_count = _voter.failover_count(); + } + + // reschedule timeout + ScheduleDelayed(100_ms); + + perf_end(_cycle_perf); +} + +void VehicleAirData::PrintStatus() +{ + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("selected barometer: %" PRIu32 " (%" PRId8 ")", _last_data[_selected_sensor_sub_index].device_id, + _selected_sensor_sub_index); + + if (fabsf(_thermal_offset[_selected_sensor_sub_index]) > 0.f) { + PX4_INFO("%" PRIu32 " temperature offset: %.4f", _last_data[_selected_sensor_sub_index].device_id, + (double)_thermal_offset[_selected_sensor_sub_index]); + } + } + + _voter.print(); +} + +}; // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/prometheus_px4/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp new file mode 100644 index 0000000..4fed84d --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "data_validator/DataValidatorGroup.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ +class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + VehicleAirData(); + ~VehicleAirData() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(); + void SensorCorrectionsUpdate(bool force = false); + void AirTemperatureUpdate(); + + static constexpr int MAX_SENSOR_COUNT = 4; + + uORB::Publication _vehicle_air_data_pub{ORB_ID(vehicle_air_data)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; + uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { + {this, ORB_ID(sensor_baro), 0}, + {this, ORB_ID(sensor_baro), 1}, + {this, ORB_ID(sensor_baro), 2}, + {this, ORB_ID(sensor_baro), 3}, + }; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + hrt_abstime _last_publication_timestamp{0}; + hrt_abstime _last_error_message{0}; + orb_advert_t _mavlink_log_pub{nullptr}; + + DataValidatorGroup _voter{1}; + unsigned _last_failover_count{0}; + + uint64_t _baro_timestamp_sum{0}; + float _baro_sum{0.f}; + int _baro_sum_count{0}; + + sensor_baro_s _last_data[MAX_SENSOR_COUNT] {}; + bool _advertised[MAX_SENSOR_COUNT] {}; + + float _thermal_offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f}; + + uint8_t _priority[MAX_SENSOR_COUNT] {}; + + int8_t _selected_sensor_sub_index{-1}; + + float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature + + DEFINE_PARAMETERS( + (ParamFloat) _param_sens_baro_qnh, + (ParamFloat) _param_sens_baro_rate + ) +}; +}; // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_air_data/params.c b/src/prometheus_px4/src/modules/sensors/vehicle_air_data/params.c new file mode 100644 index 0000000..f86bc43 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_air_data/params.c @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * QNH for barometer + * + * @min 500 + * @max 1500 + * @group Sensors + * @unit hPa + * + * @reboot_required true + * + */ +PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); + +/** + * Baro max rate. + * + * Barometric air data maximum publication rate. This is an upper bound, + * actual barometric data rate is still dependant on the sensor. + * + * @min 1 + * @max 200 + * @group Sensors + * @unit Hz + * + * @reboot_required true + * + */ +PARAM_DEFINE_FLOAT(SENS_BARO_RATE, 20.0f); diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt new file mode 100644 index 0000000..e38916f --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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(vehicle_angular_velocity + VehicleAngularVelocity.cpp + VehicleAngularVelocity.hpp +) + +target_compile_options(vehicle_angular_velocity PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +target_link_libraries(vehicle_angular_velocity + PRIVATE + mathlib + px4_work_queue + sensor_calibration +) diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp new file mode 100644 index 0000000..475a5d7 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -0,0 +1,794 @@ +/**************************************************************************** + * + * 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 "VehicleAngularVelocity.hpp" + +#include + +#include + +using namespace matrix; + +namespace sensors +{ + +VehicleAngularVelocity::VehicleAngularVelocity() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ +} + +VehicleAngularVelocity::~VehicleAngularVelocity() +{ + Stop(); + + perf_free(_filter_reset_perf); + perf_free(_selection_changed_perf); +#if !defined(CONSTRAINED_FLASH) + perf_free(_dynamic_notch_filter_esc_rpm_update_perf); + perf_free(_dynamic_notch_filter_fft_update_perf); + perf_free(_dynamic_notch_filter_esc_rpm_perf); + perf_free(_dynamic_notch_filter_fft_perf); +#endif // CONSTRAINED_FLASH +} + +bool VehicleAngularVelocity::Start() +{ + // force initial updates + ParametersUpdate(true); + + // sensor_selection needed to change the active sensor if the primary stops updating + if (!_sensor_selection_sub.registerCallback()) { + PX4_ERR("sensor_selection callback registration failed"); + return false; + } + + if (!SensorSelectionUpdate(true)) { + _sensor_sub.registerCallback(); + } + + return true; +} + +void VehicleAngularVelocity::Stop() +{ + // clear all registered callbacks + _sensor_sub.unregisterCallback(); + _sensor_fifo_sub.unregisterCallback(); + _sensor_selection_sub.unregisterCallback(); + + Deinit(); +} + +bool VehicleAngularVelocity::UpdateSampleRate() +{ + float sample_rate_hz = NAN; + float publish_rate_hz = NAN; + + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; + + if (imu_status.get().gyro_device_id == _selected_sensor_device_id) { + sample_rate_hz = imu_status.get().gyro_raw_rate_hz; + publish_rate_hz = imu_status.get().gyro_rate_hz; + break; + } + } + + // calculate sensor update rate + if ((sample_rate_hz > 0) && PX4_ISFINITE(sample_rate_hz) && (publish_rate_hz > 0) && PX4_ISFINITE(publish_rate_hz)) { + // check if sample rate error is greater than 1% + if ((_filter_sample_rate_hz <= FLT_EPSILON) || !PX4_ISFINITE(_filter_sample_rate_hz) + || (fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f) { + + PX4_DEBUG("updating sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz); + _reset_filters = true; + _filter_sample_rate_hz = sample_rate_hz; + + if (_param_imu_gyro_ratemax.get() > 0.f) { + // determine number of sensor samples that will get closest to the desired rate + const float configured_interval_us = 1e6f / _param_imu_gyro_ratemax.get(); + const float publish_interval_us = 1e6f / publish_rate_hz; + + const uint8_t samples = roundf(configured_interval_us / publish_interval_us); + + if (_fifo_available) { + _sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH)); + + } else { + _sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH)); + } + + // publish interval (constrained 100 Hz - 8 kHz) + _publish_interval_min_us = math::constrain((int)roundf(configured_interval_us - (publish_interval_us / 2.f)), 125, + 10000); + + } else { + _sensor_sub.set_required_updates(1); + _sensor_fifo_sub.set_required_updates(1); + _publish_interval_min_us = 0; + } + } + } + + return PX4_ISFINITE(_filter_sample_rate_hz) && (_filter_sample_rate_hz > 0); +} + +void VehicleAngularVelocity::ResetFilters() +{ + if ((_filter_sample_rate_hz > 0) && PX4_ISFINITE(_filter_sample_rate_hz)) { + + const Vector3f angular_velocity_uncalibrated{GetResetAngularVelocity()}; + const Vector3f angular_acceleration_uncalibrated{GetResetAngularAcceleration()}; + + for (int axis = 0; axis < 3; axis++) { + // angular velocity low pass + _lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get()); + _lp_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis)); + + // angular velocity notch + _notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(), + _param_imu_gyro_nf_bw.get()); + _notch_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis)); + + // angular acceleration low pass + _lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()); + _lp_filter_acceleration[axis].reset(angular_acceleration_uncalibrated(axis)); + } + + // force reset notch filters on any scale change + UpdateDynamicNotchEscRpm(true); + UpdateDynamicNotchFFT(true); + + _angular_velocity_raw_prev = angular_velocity_uncalibrated; + + _reset_filters = false; + perf_count(_filter_reset_perf); + } +} + +void VehicleAngularVelocity::SensorBiasUpdate(bool force) +{ + // find corresponding estimated sensor bias + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + + if (_estimator_sensor_bias_sub.updated() || force) { + estimator_sensor_bias_s bias; + + if (_estimator_sensor_bias_sub.copy(&bias) && (bias.gyro_device_id == _selected_sensor_device_id)) { + _bias = Vector3f{bias.gyro_bias}; + + } else { + _bias.zero(); + } + } +} + +bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) +{ + if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) { + sensor_selection_s sensor_selection{}; + _sensor_selection_sub.copy(&sensor_selection); + + if (_selected_sensor_device_id != sensor_selection.gyro_device_id) { + + // see if the selected sensor publishes sensor_gyro_fifo + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; + + if ((sensor_gyro_fifo_sub.get().device_id != 0) + && (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id)) { + if (_sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) { + // make sure non-FIFO sub is unregistered + _sensor_sub.unregisterCallback(); + + // record selected sensor + _selected_sensor_device_id = sensor_selection.gyro_device_id; + _calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id); + + _timestamp_sample_last = 0; + _filter_sample_rate_hz = NAN; + _reset_filters = true; + _bias.zero(); + _fifo_available = true; + + perf_count(_selection_changed_perf); + + PX4_DEBUG("selecting gyro FIFO %d %d", i, _selected_sensor_device_id); + + return true; + } + } + } + + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + + if ((sensor_gyro_sub.get().device_id != 0) + && (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id)) { + if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { + // make sure FIFO sub is unregistered + _sensor_fifo_sub.unregisterCallback(); + + // record selected sensor + _calibration.set_device_id(sensor_gyro_sub.get().device_id); + _selected_sensor_device_id = sensor_selection.gyro_device_id; + + _timestamp_sample_last = 0; + _filter_sample_rate_hz = NAN; + _reset_filters = true; + _bias.zero(); + _fifo_available = false; + + perf_count(_selection_changed_perf); + + PX4_DEBUG("selecting gyro %d %d", i, _selected_sensor_device_id); + + return true; + } + } + } + + PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", sensor_selection.gyro_device_id); + _selected_sensor_device_id = 0; + } + } + + return false; +} + +void VehicleAngularVelocity::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + + _calibration.ParametersUpdate(); + + // gyro low pass cutoff frequency changed + for (auto &lp : _lp_filter_velocity) { + if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) { + _reset_filters = true; + break; + } + } + + // gyro notch filter frequency or bandwidth changed + for (auto &nf : _notch_filter_velocity) { + if ((fabsf(nf.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f) + || (fabsf(nf.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)) { + + _reset_filters = true; + break; + } + } + + // gyro derivative low pass cutoff changed + for (auto &lp : _lp_filter_acceleration) { + if (fabsf(lp.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) { + _reset_filters = true; + break; + } + } + +#if !defined(CONSTRAINED_FLASH) + + if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm) { + if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) { + _dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter ESC RPM update"); + } + + if (_dynamic_notch_filter_esc_rpm_perf == nullptr) { + _dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter"); + } + + } else { + DisableDynamicNotchEscRpm(); + } + + if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) { + if (_dynamic_notch_filter_fft_update_perf == nullptr) { + _dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update"); + } + + if (_dynamic_notch_filter_fft_perf == nullptr) { + _dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter"); + } + + } else { + DisableDynamicNotchFFT(); + } + +#endif // !CONSTRAINED_FLASH + } +} + +Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const +{ + if (_last_publish != 0) { + // angular velocity filtering is performed on raw unscaled data + // start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection) + Vector3f angular_velocity_uncalibrated{_calibration.Uncorrect(_angular_velocity + _bias)}; + + if (PX4_ISFINITE(angular_velocity_uncalibrated(0)) + && PX4_ISFINITE(angular_velocity_uncalibrated(1)) + && PX4_ISFINITE(angular_velocity_uncalibrated(2))) { + + return angular_velocity_uncalibrated; + } + } + + return Vector3f{0.f, 0.f, 0.f}; +} + +Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const +{ + if (_last_publish != 0) { + // angular acceleration filtering is performed on unscaled angular velocity data + // start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection) + Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration}; + + if (PX4_ISFINITE(angular_acceleration(0)) + && PX4_ISFINITE(angular_acceleration(1)) + && PX4_ISFINITE(angular_acceleration(2))) { + + return angular_acceleration; + } + } + + return Vector3f{0.f, 0.f, 0.f}; +} + +void VehicleAngularVelocity::DisableDynamicNotchEscRpm() +{ +#if !defined(CONSTRAINED_FLASH) + + if (_dynamic_notch_esc_rpm_available) { + for (int axis = 0; axis < 3; axis++) { + for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) { + for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { + _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(0, 0, 0); + } + + _esc_available.set(esc, false); + } + } + + _dynamic_notch_esc_rpm_available = false; + } + +#endif // !CONSTRAINED_FLASH +} + +void VehicleAngularVelocity::DisableDynamicNotchFFT() +{ +#if !defined(CONSTRAINED_FLASH) + + if (_dynamic_notch_fft_available) { + for (int axis = 0; axis < 3; axis++) { + for (int peak = 0; peak < MAX_NUM_FFT_PEAKS; peak++) { + _dynamic_notch_filter_fft[axis][peak].setParameters(0, 0, 0); + } + } + + _dynamic_notch_fft_available = false; + } + +#endif // !CONSTRAINED_FLASH +} + +void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) +{ +#if !defined(CONSTRAINED_FLASH) + const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm; + + if (enabled && (_esc_status_sub.updated() || force)) { + + if (!_dynamic_notch_esc_rpm_available) { + // force update filters if previously disabled + force = true; + } + + esc_status_s esc_status; + + if (_esc_status_sub.copy(&esc_status) && (hrt_elapsed_time(&esc_status.timestamp) < DYNAMIC_NOTCH_FITLER_TIMEOUT)) { + + static constexpr int32_t ESC_RPM_MIN = 20 * 60; // TODO: configurable + const int32_t ESC_RPM_MAX = roundf(_filter_sample_rate_hz / 3.f * 60.f); // upper bound safety (well below Nyquist) + + for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESC_RPM); esc++) { + + const esc_report_s &esc_report = esc_status.esc[esc]; + + // only update if ESC RPM range seems valid + if ((esc_report.esc_rpm > ESC_RPM_MIN) && (esc_report.esc_rpm < ESC_RPM_MAX) + && (hrt_elapsed_time(&esc_report.timestamp) < DYNAMIC_NOTCH_FITLER_TIMEOUT)) { + + // for each ESC check determine if enabled/disabled from first notch (x axis, harmonic 0) + auto &nfx0 = _dynamic_notch_filter_esc_rpm[0][esc][0]; + + bool reset = (nfx0.getNotchFreq() <= FLT_EPSILON); // notch was previously disabled + + const float esc_hz = static_cast(esc_report.esc_rpm) / 60.f; + + // update filter parameters if frequency changed or forced + if (force || reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > FLT_EPSILON)) { + static constexpr float ESC_NOTCH_BW_HZ = 8.f; // TODO: configurable bandwidth + + // force reset if the notch frequency jumps significantly + if (!reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > ESC_NOTCH_BW_HZ)) { + reset = true; + } + + for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS; harmonic >= 0; harmonic--) { + const float frequency_hz = esc_hz * (harmonic + 1); + + for (int axis = 0; axis < 3; axis++) { + _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(_filter_sample_rate_hz, frequency_hz, ESC_NOTCH_BW_HZ); + } + } + + perf_count(_dynamic_notch_filter_esc_rpm_update_perf); + } + + if (force || reset) { + const Vector3f reset_angular_velocity{GetResetAngularVelocity()}; + + for (int axis = 0; axis < 3; axis++) { + for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { + _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].reset(reset_angular_velocity(axis)); + } + } + } + + _dynamic_notch_esc_rpm_available = true; + _esc_available.set(esc, true); + _last_esc_rpm_notch_update[esc] = esc_report.timestamp; + + } else if (force || (hrt_elapsed_time(&_last_esc_rpm_notch_update[esc]) >= DYNAMIC_NOTCH_FITLER_TIMEOUT)) { + // disable all notch filters for this ESC after timeout + _esc_available.set(esc, false); + + for (int axis = 0; axis < 3; axis++) { + for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { + _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(0, 0, 0); + } + } + } + } + + } else { + DisableDynamicNotchEscRpm(); + } + } + +#endif // !CONSTRAINED_FLASH +} + +void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) +{ +#if !defined(CONSTRAINED_FLASH) + const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT; + + if (enabled && (_sensor_gyro_fft_sub.updated() || force)) { + + if (!_dynamic_notch_fft_available) { + // force update filters if previously disabled + force = true; + } + + sensor_gyro_fft_s sensor_gyro_fft; + + if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) + && (sensor_gyro_fft.device_id == _selected_sensor_device_id) + && (hrt_elapsed_time(&sensor_gyro_fft.timestamp) < DYNAMIC_NOTCH_FITLER_TIMEOUT) + && (fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) < 10.f)) { + + // ignore any peaks below half the gyro cutoff frequency + const float peak_freq_min = _param_imu_gyro_cutoff.get() / 2.f; + const float peak_freq_max = _filter_sample_rate_hz / 3.f; // upper bound safety (well below Nyquist) + + const float bandwidth = math::constrain(sensor_gyro_fft.resolution_hz, 8.f, 30.f); // TODO: base on numerical limits? + + float *peak_frequencies[] {sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z}; + + for (int axis = 0; axis < 3; axis++) { + for (int peak = 0; peak < MAX_NUM_FFT_PEAKS; peak++) { + auto &nf = _dynamic_notch_filter_fft[axis][peak]; + + bool reset = (nf.getNotchFreq() <= FLT_EPSILON); // notch was previously disabled + + const float peak_freq = peak_frequencies[axis][peak]; + + if (PX4_ISFINITE(peak_freq) && (peak_freq > peak_freq_min) && (peak_freq < peak_freq_max)) { + // force reset if the notch frequency jumps significantly + if (fabsf(nf.getNotchFreq() - peak_freq) > bandwidth) { + reset = true; + } + + // update filter parameters if frequency changed or forced + if (force || (fabsf(nf.getNotchFreq() - peak_freq) > FLT_EPSILON)) { + nf.setParameters(_filter_sample_rate_hz, peak_freq, bandwidth); + perf_count(_dynamic_notch_filter_fft_update_perf); + } + + if (force || reset) { + const Vector3f reset_angular_velocity{GetResetAngularVelocity()}; + nf.reset(reset_angular_velocity(axis)); + } + + _dynamic_notch_fft_available = true; + + } else { + // disable this notch filter (if it isn't already) + if (force || !reset) { + nf.setParameters(0, 0, 0); + } + } + } + } + + } else { + DisableDynamicNotchFFT(); + } + } + +#endif // !CONSTRAINED_FLASH +} + +float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int N) +{ +#if !defined(CONSTRAINED_FLASH) + + // Apply dynamic notch filter from ESC RPM + if (_dynamic_notch_esc_rpm_available) { + perf_begin(_dynamic_notch_filter_esc_rpm_perf); + + for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) { + if (_esc_available[esc]) { + for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS - 1; harmonic >= 0; harmonic--) { + _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].applyArray(data, N); + } + } + } + + perf_end(_dynamic_notch_filter_esc_rpm_perf); + } + + // Apply dynamic notch filter from FFT + if (_dynamic_notch_fft_available) { + perf_begin(_dynamic_notch_filter_fft_perf); + + for (int peak = MAX_NUM_FFT_PEAKS - 1; peak >= 0; peak--) { + if (_dynamic_notch_filter_fft[axis][peak].getNotchFreq() > 0.f) { + _dynamic_notch_filter_fft[axis][peak].applyArray(data, N); + } + } + + perf_end(_dynamic_notch_filter_fft_perf); + } + +#endif // !CONSTRAINED_FLASH + + // Apply general notch filter (IMU_GYRO_NF_FREQ) + if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) { + _notch_filter_velocity[axis].applyArray(data, N); + } + + // Apply general low-pass filter (IMU_GYRO_CUTOFF) + _lp_filter_velocity[axis].applyArray(data, N); + + // return last filtered sample + return data[N - 1]; +} + +float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float dt_s, float data[], int N) +{ + // angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + float angular_acceleration_filtered = 0.f; + + for (int n = 0; n < N; n++) { + const float angular_acceleration = (data[n] - _angular_velocity_raw_prev(axis)) / dt_s; + angular_acceleration_filtered = _lp_filter_acceleration[axis].apply(angular_acceleration); + _angular_velocity_raw_prev(axis) = data[n]; + } + + return angular_acceleration_filtered; +} + +void VehicleAngularVelocity::Run() +{ + // backup schedule + ScheduleDelayed(10_ms); + + ParametersUpdate(); + + // update corrections first to set _selected_sensor + const bool selection_updated = SensorSelectionUpdate(); + + _calibration.SensorCorrectionsUpdate(selection_updated); + SensorBiasUpdate(selection_updated); + + if (selection_updated || !PX4_ISFINITE(_filter_sample_rate_hz) || (_filter_sample_rate_hz <= FLT_EPSILON)) { + if (!UpdateSampleRate()) { + // sensor sample rate required to run + return; + } + } + + if (_fifo_available) { + // process all outstanding fifo messages + sensor_gyro_fifo_s sensor_fifo_data; + + while (_sensor_fifo_sub.update(&sensor_fifo_data)) { + const float dt_s = math::constrain(sensor_fifo_data.dt * 1e-6f, 0.00002f, 0.02f); // 20 us to 20 ms + + // in FIFO mode the unscaled raw data is filtered, reset filters on any scale change + if (_reset_filters) { + ResetFilters(); + + if (_reset_filters) { + continue; // not safe to run until filters configured + } + } + + UpdateDynamicNotchEscRpm(); + UpdateDynamicNotchFFT(); + + const int N = sensor_fifo_data.samples; + static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); + + if ((N > 0) && (N <= FIFO_SIZE_MAX)) { + Vector3f angular_velocity_unscaled; + Vector3f angular_acceleration_unscaled; + + int16_t *raw_data_array[] {sensor_fifo_data.x, sensor_fifo_data.y, sensor_fifo_data.z}; + + for (int axis = 0; axis < 3; axis++) { + // copy raw int16 sensor samples to float array for filtering + float data[FIFO_SIZE_MAX]; + + for (int n = 0; n < N; n++) { + data[n] = sensor_fifo_data.scale * raw_data_array[axis][n]; + } + + // save last filtered sample + angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, N); + angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, dt_s, data, N); + } + + // Publish + CalibrateAndPublish(!_sensor_fifo_sub.updated(), sensor_fifo_data.timestamp_sample, angular_velocity_unscaled, + angular_acceleration_unscaled); + } + } + + } else { + // process all outstanding messages + sensor_gyro_s sensor_data; + + while (_sensor_sub.update(&sensor_data)) { + if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) { + + if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) { + _timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz; + } + + const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), 0.00002f, 0.02f); + _timestamp_sample_last = sensor_data.timestamp_sample; + + if (_reset_filters) { + ResetFilters(); + + if (_reset_filters) { + continue; // not safe to run until filters configured + } + } + + UpdateDynamicNotchEscRpm(); + UpdateDynamicNotchFFT(); + + Vector3f angular_velocity; + Vector3f angular_acceleration; + + float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z}; + + for (int axis = 0; axis < 3; axis++) { + // copy sensor sample to float array for filtering + float data[1] {raw_data_array[axis]}; + + // save last filtered sample + angular_velocity(axis) = FilterAngularVelocity(axis, data); + angular_acceleration(axis) = FilterAngularAcceleration(axis, dt_s, data); + } + + // Publish + CalibrateAndPublish(!_sensor_sub.updated(), sensor_data.timestamp_sample, angular_velocity, angular_acceleration); + } + } + } +} + +void VehicleAngularVelocity::CalibrateAndPublish(bool publish, const hrt_abstime ×tamp_sample, + const Vector3f &angular_velocity_unscaled, const Vector3f &angular_acceleration_unscaled) +{ + // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias + _angular_velocity_prev = _angular_velocity; + _angular_velocity = _calibration.Correct(angular_velocity_unscaled) - _bias; + + // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation + _angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled; + + if (publish && (timestamp_sample >= _last_publish + _publish_interval_min_us)) { + + // Publish vehicle_angular_acceleration + vehicle_angular_acceleration_s v_angular_acceleration; + v_angular_acceleration.timestamp_sample = timestamp_sample; + _angular_acceleration.copyTo(v_angular_acceleration.xyz); + v_angular_acceleration.timestamp = hrt_absolute_time(); + _vehicle_angular_acceleration_pub.publish(v_angular_acceleration); + + // Publish vehicle_angular_velocity + vehicle_angular_velocity_s v_angular_velocity; + v_angular_velocity.timestamp_sample = timestamp_sample; + _angular_velocity.copyTo(v_angular_velocity.xyz); + v_angular_velocity.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_pub.publish(v_angular_velocity); + + // shift last publish time forward, but don't let it get further behind than the interval + _last_publish = math::constrain(_last_publish + _publish_interval_min_us, + timestamp_sample - _publish_interval_min_us, timestamp_sample); + } +} + +void VehicleAngularVelocity::PrintStatus() +{ + PX4_INFO("selected sensor: %" PRIu32 ", rate: %.1f Hz %s", + _selected_sensor_device_id, (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : ""); + PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2)); + + _calibration.PrintStatus(); +} + +} // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp new file mode 100644 index 0000000..6b893a6 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ + +class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + VehicleAngularVelocity(); + ~VehicleAngularVelocity() override; + + void PrintStatus(); + bool Start(); + void Stop(); + +private: + void Run() override; + + void CalibrateAndPublish(bool publish, const hrt_abstime ×tamp_sample, const matrix::Vector3f &angular_velocity, + const matrix::Vector3f &angular_acceleration); + + inline float FilterAngularVelocity(int axis, float data[], int N = 1); + inline float FilterAngularAcceleration(int axis, float dt_s, float data[], int N = 1); + + void DisableDynamicNotchEscRpm(); + void DisableDynamicNotchFFT(); + void ParametersUpdate(bool force = false); + + void ResetFilters(); + void SensorBiasUpdate(bool force = false); + bool SensorSelectionUpdate(bool force = false); + void UpdateDynamicNotchEscRpm(bool force = false); + void UpdateDynamicNotchFFT(bool force = false); + bool UpdateSampleRate(); + + // scaled appropriately for current sensor + matrix::Vector3f GetResetAngularVelocity() const; + matrix::Vector3f GetResetAngularAcceleration() const; + + static constexpr int MAX_SENSOR_COUNT = 4; + + uORB::Publication _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)}; + uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; + + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; +#if !defined(CONSTRAINED_FLASH) + uORB::Subscription _esc_status_sub {ORB_ID(esc_status)}; + uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)}; +#endif // !CONSTRAINED_FLASH + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; + uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)}; + uORB::SubscriptionCallbackWorkItem _sensor_fifo_sub{this, ORB_ID(sensor_gyro_fifo)}; + + calibration::Gyroscope _calibration{}; + + matrix::Vector3f _bias{}; + + matrix::Vector3f _angular_velocity{}; + matrix::Vector3f _angular_velocity_prev{}; + matrix::Vector3f _angular_acceleration{}; + + matrix::Vector3f _angular_velocity_raw_prev{}; + hrt_abstime _timestamp_sample_last{0}; + + hrt_abstime _publish_interval_min_us{0}; + hrt_abstime _last_publish{0}; + + float _filter_sample_rate_hz{NAN}; + + // angular velocity filters + math::LowPassFilter2p _lp_filter_velocity[3] {}; + math::NotchFilter _notch_filter_velocity[3] {}; + +#if !defined(CONSTRAINED_FLASH) + + enum DynamicNotch { + EscRpm = 1, + FFT = 2, + }; + + static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 1_s; + + static constexpr int MAX_NUM_ESC_RPM = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]); + static constexpr int MAX_NUM_ESC_RPM_HARMONICS = 3; + + static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof( + sensor_gyro_fft_s::peak_frequencies_x[0]); + + math::NotchFilter _dynamic_notch_filter_esc_rpm[3][MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS] {}; + math::NotchFilter _dynamic_notch_filter_fft[3][MAX_NUM_FFT_PEAKS] {}; + + px4::Bitset _esc_available{}; + hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESC_RPM] {}; + + perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_esc_rpm_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_fft_perf{nullptr}; + + bool _dynamic_notch_esc_rpm_available{false}; + bool _dynamic_notch_fft_available{false}; +#endif // !CONSTRAINED_FLASH + + // angular acceleration filter + math::LowPassFilter2p _lp_filter_acceleration[3] {}; + + uint32_t _selected_sensor_device_id{0}; + + bool _reset_filters{true}; + bool _fifo_available{false}; + + perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro filter reset")}; + perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro selection changed")}; + + DEFINE_PARAMETERS( +#if !defined(CONSTRAINED_FLASH) + (ParamInt) _param_imu_gyro_dyn_nf, +#endif // !CONSTRAINED_FLASH + (ParamFloat) _param_imu_gyro_cutoff, + (ParamFloat) _param_imu_gyro_nf_freq, + (ParamFloat) _param_imu_gyro_nf_bw, + (ParamInt) _param_imu_gyro_ratemax, + (ParamFloat) _param_imu_dgyro_cutoff + ) +}; + +} // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c new file mode 100644 index 0000000..0a24c40 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** +* Notch filter frequency for gyro +* +* The center frequency for the 2nd order notch filter on the primary gyro. +* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency. +* This only affects the signal sent to the controllers, not the estimators. +* Applies to both angular velocity and angular acceleration sent to the controllers. +* See "IMU_GYRO_NF_BW" to set the bandwidth of the filter. +* +* A value of 0 disables the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f); + +/** +* Notch filter bandwidth for gyro +* +* The frequency width of the stop band for the 2nd order notch filter on the primary gyro. +* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency. +* Applies to both angular velocity and angular acceleration sent to the controllers. +* +* @min 0 +* @max 100 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f); + +/** +* Low pass filter cutoff frequency for gyro +* +* The cutoff frequency for the 2nd order butterworth filter on the primary gyro. +* This only affects the angular velocity sent to the controllers, not the estimators. +* It applies also to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF. +* +* A value of 0 disables the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 40.0f); + +/** +* Gyro control data maximum publication rate (inner loop rate) +* +* The maximum rate the gyro control data (vehicle_angular_velocity) will be +* allowed to publish at. This is the loop rate for the rate controller and outputs. +* +* Note: sensor data is always read and filtered at the full raw rate (eg commonly 8 kHz) regardless of this setting. +* +* @min 100 +* @max 2000 +* @value 100 100 Hz +* @value 250 250 Hz +* @value 400 400 Hz +* @value 800 800 Hz +* @value 1000 1000 Hz +* @value 2000 2000 Hz +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); + +/** +* Cutoff frequency for angular acceleration (D-Term filter) +* +* The cutoff frequency for the 2nd order butterworth filter used on +* the time derivative of the measured angular velocity, also known as +* the D-term filter in the rate controller. The D-term uses the derivative of +* the rate and thus is the most susceptible to noise. Therefore, using +* a D-term filter allows to increase IMU_GYRO_CUTOFF, which +* leads to reduced control latency and permits to increase the P gains. +* +* A value of 0 disables the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); + +/** +* IMU gyro dynamic notch filtering +* +* Enable bank of dynamically updating notch filters. +* Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN). +* @group Sensors +* @category Developer +* @min 0 +* @max 3 +* @bit 0 ESC RPM +* @bit 1 FFT +*/ +PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0); diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/CMakeLists.txt new file mode 100644 index 0000000..ab5be00 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(vehicle_gps_position + VehicleGPSPosition.cpp + VehicleGPSPosition.hpp + gps_blending.cpp + gps_blending.hpp +) +target_link_libraries(vehicle_gps_position PRIVATE ecl_geo px4_work_queue) + +px4_add_functional_gtest(SRC gps_blending_test.cpp LINKLIBS vehicle_gps_position ecl_geo) diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp new file mode 100644 index 0000000..dfd703c --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * 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 "VehicleGPSPosition.hpp" + +#include +#include +#include + +namespace sensors +{ +VehicleGPSPosition::VehicleGPSPosition() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ +} + +VehicleGPSPosition::~VehicleGPSPosition() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleGPSPosition::Start() +{ + // force initial updates + ParametersUpdate(true); + + ScheduleNow(); + + return true; +} + +void VehicleGPSPosition::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_gps_sub) { + sub.unregisterCallback(); + } +} + +void VehicleGPSPosition::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + + if (_param_sens_gps_mask.get() == 0) { + _sensor_gps_sub[0].registerCallback(); + + } else { + for (auto &sub : _sensor_gps_sub) { + sub.registerCallback(); + } + } + + _gps_blending.setBlendingUseSpeedAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC); + _gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC); + _gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC); + _gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get()); + _gps_blending.setPrimaryInstance(_param_sens_gps_prime.get()); + } +} + +void VehicleGPSPosition::Run() +{ + perf_begin(_cycle_perf); + ParametersUpdate(); + + // GPS blending + ScheduleDelayed(500_ms); // backup schedule + + // Check all GPS instance + bool any_gps_updated = false; + bool gps_updated = false; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + gps_updated = _sensor_gps_sub[i].updated(); + + sensor_gps_s gps_data; + + if (gps_updated) { + any_gps_updated = true; + + _sensor_gps_sub[i].copy(&gps_data); + _gps_blending.setGpsData(gps_data, i); + + if (!_sensor_gps_sub[i].registered()) { + _sensor_gps_sub[i].registerCallback(); + } + } + } + + if (any_gps_updated) { + _gps_blending.update(hrt_absolute_time()); + + if (_gps_blending.isNewOutputDataAvailable()) { + Publish(_gps_blending.getOutputGpsData(), _gps_blending.getSelectedGps()); + } + } + + perf_end(_cycle_perf); +} + +void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected) +{ + vehicle_gps_position_s gps_output{}; + + gps_output.timestamp = gps.timestamp; + gps_output.time_utc_usec = gps.time_utc_usec; + gps_output.lat = gps.lat; + gps_output.lon = gps.lon; + gps_output.alt = gps.alt; + gps_output.alt_ellipsoid = gps.alt_ellipsoid; + gps_output.s_variance_m_s = gps.s_variance_m_s; + gps_output.c_variance_rad = gps.c_variance_rad; + gps_output.eph = gps.eph; + gps_output.epv = gps.epv; + gps_output.hdop = gps.hdop; + gps_output.vdop = gps.vdop; + gps_output.noise_per_ms = gps.noise_per_ms; + gps_output.jamming_indicator = gps.jamming_indicator; + gps_output.jamming_state = gps.jamming_state; + gps_output.vel_m_s = gps.vel_m_s; + gps_output.vel_n_m_s = gps.vel_n_m_s; + gps_output.vel_e_m_s = gps.vel_e_m_s; + gps_output.vel_d_m_s = gps.vel_d_m_s; + gps_output.cog_rad = gps.cog_rad; + gps_output.timestamp_time_relative = gps.timestamp_time_relative; + gps_output.heading = gps.heading; + gps_output.heading_offset = gps.heading_offset; + gps_output.fix_type = gps.fix_type; + gps_output.vel_ned_valid = gps.vel_ned_valid; + gps_output.satellites_used = gps.satellites_used; + + gps_output.selected = selected; + + _vehicle_gps_position_pub.publish(gps_output); +} + +void VehicleGPSPosition::PrintStatus() +{ + //PX4_INFO("selected GPS: %d", _gps_select_index); +} + +}; // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp new file mode 100644 index 0000000..8126fd5 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gps_blending.hpp" + +using namespace time_literals; + +namespace sensors +{ +class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + VehicleGPSPosition(); + ~VehicleGPSPosition() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(bool force = false); + void Publish(const sensor_gps_s &gps, uint8_t selected); + + // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm + static constexpr uint8_t BLEND_MASK_USE_SPD_ACC = 1; + static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2; + static constexpr uint8_t BLEND_MASK_USE_VPOS_ACC = 4; + + // define max number of GPS receivers supported + static constexpr int GPS_MAX_RECEIVERS = 2; + static_assert(GPS_MAX_RECEIVERS == GpsBlending::GPS_MAX_RECEIVERS_BLEND, + "GPS_MAX_RECEIVERS must match to GPS_MAX_RECEIVERS_BLEND"); + + uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _sensor_gps_sub[GPS_MAX_RECEIVERS] { /**< sensor data subscription */ + {this, ORB_ID(sensor_gps), 0}, + {this, ORB_ID(sensor_gps), 1}, + }; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + GpsBlending _gps_blending; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_gps_mask, + (ParamFloat) _param_sens_gps_tau, + (ParamInt) _param_sens_gps_prime + ) +}; +}; // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending.cpp new file mode 100644 index 0000000..72ab461 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -0,0 +1,593 @@ +/**************************************************************************** + * + * 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 gps_blending.cpp + */ + +#include "gps_blending.hpp" + + +void GpsBlending::update(uint64_t hrt_now_us) +{ + _is_new_output_data_available = false; + + // blend multiple receivers if available + if (!blend_gps_data(hrt_now_us)) { + // Only use selected receiver data if it has been updated + uint8_t gps_select_index = 0; + + // Find the single "best" GPS from the data we have + // First, find the GPS(s) with the best fix + uint8_t best_fix = 0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].fix_type > best_fix) { + best_fix = _gps_state[i].fix_type; + } + } + + // Second, compare GPS's with best fix and take the one with most satellites + uint8_t max_sats = 0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].fix_type == best_fix && _gps_state[i].satellites_used > max_sats) { + max_sats = _gps_state[i].satellites_used; + gps_select_index = i; + } + } + + // Check for new data on selected GPS, and clear blend offsets + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + _NE_pos_offset_m[i].zero(); + _hgt_offset_mm[i] = 0.0f; + } + + // Only use a secondary instance if the fallback is allowed + if ((_primary_instance > -1) + && (gps_select_index != _primary_instance) + && !_fallback_allowed) { + gps_select_index = _primary_instance; + } + + _selected_gps = gps_select_index; + _is_new_output_data_available = _gps_updated[gps_select_index]; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + _gps_updated[gps_select_index] = false; + } + } +} + +bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) +{ + /* + * If both receivers have the same update rate, use the oldest non-zero time. + * If two receivers with different update rates are used, use the slowest. + * If time difference is excessive, use newest to prevent a disconnected receiver + * from blocking updates. + */ + + // Calculate the time step for each receiver with some filtering to reduce the effects of jitter + // Find the largest and smallest time step. + float dt_max = 0.0f; + float dt_min = GPS_TIMEOUT_S; + _np_gps_suitable_for_blending = 0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + const float raw_dt = 1e-6f * ((float)_gps_state[i].timestamp - (float)_time_prev_us[i]); + const float present_dt = 1e-6f * ((float)hrt_now_us - (float)_gps_state[i].timestamp); + + if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) { + _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i]; + + } else if ((present_dt >= GPS_TIMEOUT_S) + && (_gps_state[i].timestamp > 0)) { + // Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt + _gps_state[i].timestamp = 0; + _gps_state[i].fix_type = 0; + _gps_state[i].satellites_used = 0; + _gps_state[i].vel_ned_valid = 0; + + if (i == _primary_instance) { + // Allow using a secondary instance when the primary + // receiver has timed out + _fallback_allowed = true; + } + + continue; + } + + // Only count GPSs with at least a 2D fix for blending purposes + if (_gps_state[i].fix_type < 2) { + continue; + } + + if (_gps_dt[i] > dt_max) { + dt_max = _gps_dt[i]; + _gps_slowest_index = i; + } + + if (_gps_dt[i] < dt_min) { + dt_min = _gps_dt[i]; + } + + _np_gps_suitable_for_blending++; + } + + // Find the receiver that is last be updated + uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message + uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + // Find largest and smallest times + if (_gps_state[i].timestamp > max_us) { + max_us = _gps_state[i].timestamp; + _gps_newest_index = i; + } + + if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) { + min_us = _gps_state[i].timestamp; + } + } + + if (_np_gps_suitable_for_blending < 2) { + // Less than 2 receivers left, so fall out of blending + return false; + } + + /* + * If the largest dt is less than 20% greater than the smallest, then we have receivers + * running at the same rate then we wait until we have two messages with an arrival time + * difference that is less than 50% of the smallest time step and use the time stamp from + * the newest data. + * Else we have two receivers at different update rates and use the slowest receiver + * as the timing reference. + */ + bool gps_new_output_data = false; + + if ((dt_max - dt_min) < 0.2f * dt_min) { + // both receivers assumed to be running at the same rate + if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) { + // data arrival within a short time window enables the two measurements to be blended + _gps_time_ref_index = _gps_newest_index; + gps_new_output_data = true; + } + + } else { + // both receivers running at different rates + _gps_time_ref_index = _gps_slowest_index; + + if (_gps_state[_gps_time_ref_index].timestamp > _time_prev_us[_gps_time_ref_index]) { + // blend data at the rate of the slower receiver + gps_new_output_data = true; + } + } + + if (gps_new_output_data) { + // calculate the sum squared speed accuracy across all GPS sensors + float speed_accuracy_sum_sq = 0.0f; + + if (_blend_use_spd_acc) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s > 0.0f) { + speed_accuracy_sum_sq += _gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s; + } + } + } + + // calculate the sum squared horizontal position accuracy across all GPS sensors + float horizontal_accuracy_sum_sq = 0.0f; + + if (_blend_use_hpos_acc) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) { + horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph; + } + } + } + + // calculate the sum squared vertical position accuracy across all GPS sensors + float vertical_accuracy_sum_sq = 0.0f; + + if (_blend_use_vpos_acc) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) { + vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv; + } + } + } + + // Check if we can do blending using reported accuracy + bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f + || speed_accuracy_sum_sq > 0.0f); + + // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead + if (!can_do_blending) { + return false; + } + + float sum_of_all_weights = 0.0f; + + // calculate a weighting using the reported speed accuracy + float spd_blend_weights[GPS_MAX_RECEIVERS_BLEND] {}; + + if (speed_accuracy_sum_sq > 0.0f && _blend_use_spd_acc) { + // calculate the weights using the inverse of the variances + float sum_of_spd_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s >= 0.001f) { + spd_blend_weights[i] = 1.0f / (_gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s); + sum_of_spd_weights += spd_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_spd_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights; + } + + sum_of_all_weights += 1.0f; + } + } + + // calculate a weighting using the reported horizontal position + float hpos_blend_weights[GPS_MAX_RECEIVERS_BLEND] {}; + + if (horizontal_accuracy_sum_sq > 0.0f && _blend_use_hpos_acc) { + // calculate the weights using the inverse of the variances + float sum_of_hpos_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) { + hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph); + sum_of_hpos_weights += hpos_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_hpos_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights; + } + + sum_of_all_weights += 1.0f; + } + } + + // calculate a weighting using the reported vertical position accuracy + float vpos_blend_weights[GPS_MAX_RECEIVERS_BLEND] = {}; + + if (vertical_accuracy_sum_sq > 0.0f && _blend_use_vpos_acc) { + // calculate the weights using the inverse of the variances + float sum_of_vpos_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) { + vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv); + sum_of_vpos_weights += vpos_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_vpos_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights; + } + + sum_of_all_weights += 1.0f; + }; + } + + // blend weight for each GPS. The blend weights must sum to 1.0 across all instances. + float blend_weights[GPS_MAX_RECEIVERS_BLEND]; + + // calculate an overall weight + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights; + } + + // With updated weights we can calculate a blended GPS solution and + // offsets for each physical receiver + sensor_gps_s gps_blended_state = gps_blend_states(blend_weights); + + update_gps_offsets(gps_blended_state); + + // calculate a blended output from the offset corrected receiver data + // publish if blending was successful + calc_gps_blend_output(gps_blended_state, blend_weights); + + _gps_blended_state = gps_blended_state; + _selected_gps = GPS_MAX_RECEIVERS_BLEND; + _is_new_output_data_available = true; + } + + return true; +} + +sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const +{ + // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. + sensor_gps_s gps_blended_state{}; + gps_blended_state.eph = FLT_MAX; + gps_blended_state.epv = FLT_MAX; + gps_blended_state.s_variance_m_s = FLT_MAX; + gps_blended_state.vel_ned_valid = true; + gps_blended_state.hdop = FLT_MAX; + gps_blended_state.vdop = FLT_MAX; + + // combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights() + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + // blend the timing data + gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]); + + // use the highest status + if (_gps_state[i].fix_type > gps_blended_state.fix_type) { + gps_blended_state.fix_type = _gps_state[i].fix_type; + } + + // Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers + // If any receiver contributing has an invalid velocity, then report blended velocity as invalid + if (blend_weights[i] > 0.0f) { + + // calculate a blended average speed and velocity vector + gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * blend_weights[i]; + gps_blended_state.vel_n_m_s += _gps_state[i].vel_n_m_s * blend_weights[i]; + gps_blended_state.vel_e_m_s += _gps_state[i].vel_e_m_s * blend_weights[i]; + gps_blended_state.vel_d_m_s += _gps_state[i].vel_d_m_s * blend_weights[i]; + + if (_gps_state[i].eph > 0.0f + && _gps_state[i].eph < gps_blended_state.eph) { + gps_blended_state.eph = _gps_state[i].eph; + } + + if (_gps_state[i].epv > 0.0f + && _gps_state[i].epv < gps_blended_state.epv) { + gps_blended_state.epv = _gps_state[i].epv; + } + + if (_gps_state[i].s_variance_m_s > 0.0f + && _gps_state[i].s_variance_m_s < gps_blended_state.s_variance_m_s) { + gps_blended_state.s_variance_m_s = _gps_state[i].s_variance_m_s; + } + + if (_gps_state[i].hdop > 0 + && _gps_state[i].hdop < gps_blended_state.hdop) { + gps_blended_state.hdop = _gps_state[i].hdop; + } + + if (_gps_state[i].vdop > 0 + && _gps_state[i].vdop < gps_blended_state.vdop) { + gps_blended_state.vdop = _gps_state[i].vdop; + } + + if (_gps_state[i].satellites_used > 0 + && _gps_state[i].satellites_used > gps_blended_state.satellites_used) { + gps_blended_state.satellites_used = _gps_state[i].satellites_used; + } + + if (!_gps_state[i].vel_ned_valid) { + gps_blended_state.vel_ned_valid = false; + } + } + + // TODO read parameters for individual GPS antenna positions and blend + // Vector3f temp_antenna_offset = _antenna_offset[i]; + // temp_antenna_offset *= blend_weights[i]; + // _blended_antenna_offset += temp_antenna_offset; + } + + /* + * Calculate the instantaneous weighted average location using available GPS instances and store in _gps_state. + * This is statistically the most likely location, but may not be stable enough for direct use by the EKF. + */ + + // Use the GPS with the highest weighting as the reference position + float best_weight = 0.0f; + + // index of the physical receiver with the lowest reported error + uint8_t gps_best_index = 0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (blend_weights[i] > best_weight) { + best_weight = blend_weights[i]; + gps_best_index = i; + gps_blended_state.lat = _gps_state[i].lat; + gps_blended_state.lon = _gps_state[i].lon; + gps_blended_state.alt = _gps_state[i].alt; + } + } + + // Convert each GPS position to a local NEU offset relative to the reference position + Vector2f blended_NE_offset_m{0, 0}; + float blended_alt_offset_mm = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if ((blend_weights[i] > 0.0f) && (i != gps_best_index)) { + // calculate the horizontal offset + Vector2f horiz_offset{}; + get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), + (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + &horiz_offset(0), &horiz_offset(1)); + + // sum weighted offsets + blended_NE_offset_m += horiz_offset * blend_weights[i]; + + // calculate vertical offset + float vert_offset = (float)(_gps_state[i].alt - gps_blended_state.alt); + + // sum weighted offsets + blended_alt_offset_mm += vert_offset * blend_weights[i]; + } + } + + // Add the sum of weighted offsets to the reference position to obtain the blended position + const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7; + const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7; + double lat_deg_res = 0; + double lon_deg_res = 0; + add_vector_to_global_position(lat_deg_now, lon_deg_now, + blended_NE_offset_m(0), blended_NE_offset_m(1), + &lat_deg_res, &lon_deg_res); + gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); + gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); + gps_blended_state.alt += (int32_t)blended_alt_offset_mm; + + // Take GPS heading from the highest weighted receiver that is publishing a valid .heading value + uint8_t gps_best_yaw_index = 0; + float best_yaw_weight = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (PX4_ISFINITE(_gps_state[i].heading) && (blend_weights[i] > best_yaw_weight)) { + best_yaw_weight = blend_weights[i]; + gps_best_yaw_index = i; + } + } + + gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading; + gps_blended_state.heading_offset = _gps_state[gps_best_yaw_index].heading_offset; + + return gps_blended_state; +} + +void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) +{ + // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset + // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering + float alpha[GPS_MAX_RECEIVERS_BLEND] {}; + float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f); + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].timestamp - _time_prev_us[i] > 0) { + // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter + alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]), + 0.0f, 1.0f); + + _time_prev_us[i] = _gps_state[i].timestamp; + } + } + + // Calculate a filtered position delta for each GPS relative to the blended solution state + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + Vector2f offset; + get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + (gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), + &offset(0), &offset(1)); + + _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); + + _hgt_offset_mm[i] = (float)(gps_blended_state.alt - _gps_state[i].alt) * alpha[i] + + _hgt_offset_mm[i] * (1.0f - alpha[i]); + } + + // calculate offset limits from the largest difference between receivers + Vector2f max_ne_offset{}; + float max_alt_offset = 0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + for (uint8_t j = i; j < GPS_MAX_RECEIVERS_BLEND; j++) { + if (i != j) { + Vector2f offset; + get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), + &offset(0), &offset(1)); + max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0))); + max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1))); + max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt))); + } + } + } + + // apply offset limits + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + _NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0)); + _NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1)); + _hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset); + } +} + +void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, + float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const +{ + // Convert each GPS position to a local NEU offset relative to the reference position + // which is defined as the positon of the blended solution calculated from non offset corrected data + Vector2f blended_NE_offset_m{0, 0}; + float blended_alt_offset_mm = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (blend_weights[i] > 0.0f) { + + // Add the sum of weighted offsets to the reference position to obtain the blended position + const double lat_deg_orig = (double)_gps_state[i].lat * 1.0e-7; + const double lon_deg_orig = (double)_gps_state[i].lon * 1.0e-7; + double lat_deg_offset_res = 0; + double lon_deg_offset_res = 0; + add_vector_to_global_position(lat_deg_orig, lon_deg_orig, + _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), + &lat_deg_offset_res, &lon_deg_offset_res); + + float alt_offset = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i]; + + + // calculate the horizontal offset + Vector2f horiz_offset{}; + get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), + lat_deg_offset_res, lon_deg_offset_res, + &horiz_offset(0), &horiz_offset(1)); + + // sum weighted offsets + blended_NE_offset_m += horiz_offset * blend_weights[i]; + + // calculate vertical offset + float vert_offset = alt_offset - gps_blended_state.alt; + + // sum weighted offsets + blended_alt_offset_mm += vert_offset * blend_weights[i]; + } + } + + // Add the sum of weighted offsets to the reference position to obtain the blended position + const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7; + const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7; + double lat_deg_res = 0; + double lon_deg_res = 0; + add_vector_to_global_position(lat_deg_now, lon_deg_now, + blended_NE_offset_m(0), blended_NE_offset_m(1), + &lat_deg_res, &lon_deg_res); + + gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); + gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); + gps_blended_state.alt = gps_blended_state.alt + (int32_t)blended_alt_offset_mm; +} diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending.hpp new file mode 100644 index 0000000..e137dd9 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * 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 gps_blending.hpp + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +using matrix::Vector2f; +using math::constrain; + +using namespace time_literals; + +class GpsBlending +{ +public: + // Set the GPS timeout to 2s, after which a receiver will be ignored + static constexpr hrt_abstime GPS_TIMEOUT_US = 2_s; + static constexpr float GPS_TIMEOUT_S = (GPS_TIMEOUT_US / 1e6f); + + + GpsBlending() = default; + ~GpsBlending() = default; + + // define max number of GPS receivers supported for blending + static constexpr int GPS_MAX_RECEIVERS_BLEND = 2; + + void setGpsData(const sensor_gps_s &gps_data, int instance) + { + if (instance < GPS_MAX_RECEIVERS_BLEND) { + _gps_state[instance] = gps_data; + _gps_updated[instance] = true; + } + } + void setBlendingUseSpeedAccuracy(bool enabled) { _blend_use_spd_acc = enabled; } + void setBlendingUseHPosAccuracy(bool enabled) { _blend_use_hpos_acc = enabled; } + void setBlendingUseVPosAccuracy(bool enabled) { _blend_use_vpos_acc = enabled; } + void setBlendingTimeConstant(float tau) { _blending_time_constant = tau; } + void setPrimaryInstance(int primary) { _primary_instance = primary; } + + void update(uint64_t hrt_now_us); + + bool isNewOutputDataAvailable() const { return _is_new_output_data_available; } + int getNumberOfGpsSuitableForBlending() const { return _np_gps_suitable_for_blending; } + const sensor_gps_s &getOutputGpsData() const + { + if (_selected_gps < GPS_MAX_RECEIVERS_BLEND) { + return _gps_state[_selected_gps]; + + } else { + return _gps_blended_state; + } + } + int getSelectedGps() const { return _selected_gps; } + +private: + /* + * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical + * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers + * have significant position differences, variation in receiver estimated accuracy will cause undesirable + * variation in the position solution. + */ + bool blend_gps_data(uint64_t hrt_now_us); + + /* + * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated + * by calc_blend_weights() + */ + sensor_gps_s gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const; + + /* + * The location in gps_blended_state will move around as the relative accuracy changes. + * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is + * calculated. + */ + void update_gps_offsets(const sensor_gps_s &gps_blended_state); + + /* + Calculate GPS output that is a blend of the offset corrected physical receiver data + */ + void calc_gps_blend_output(sensor_gps_s &gps_blended_state, float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const; + + sensor_gps_s _gps_state[GPS_MAX_RECEIVERS_BLEND] {}; ///< internal state data for the physical GPS + sensor_gps_s _gps_blended_state {}; + bool _gps_updated[GPS_MAX_RECEIVERS_BLEND] {}; + int _selected_gps{0}; + int _np_gps_suitable_for_blending{0}; + int _primary_instance{0}; ///< if -1, there is no primary isntance and the best receiver is used // TODO: use device_id + bool _fallback_allowed{false}; + + bool _is_new_output_data_available{false}; + + matrix::Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) + float _hgt_offset_mm[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) + + uint64_t _time_prev_us[GPS_MAX_RECEIVERS_BLEND] {}; ///< the previous value of time_us for that GPS instance - used to detect new data. + uint8_t _gps_time_ref_index{0}; ///< index of the receiver that is used as the timing reference for the blending update + uint8_t _gps_newest_index{0}; ///< index of the physical receiver with the newest data + uint8_t _gps_slowest_index{0}; ///< index of the physical receiver with the slowest update rate + float _gps_dt[GPS_MAX_RECEIVERS_BLEND] {}; ///< average time step in seconds. + + bool _blend_use_spd_acc{false}; + bool _blend_use_hpos_acc{false}; + bool _blend_use_vpos_acc{false}; + + float _blending_time_constant{0.f}; +}; diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp new file mode 100644 index 0000000..8d46073 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -0,0 +1,272 @@ +/**************************************************************************** + * + * 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 GPS blending logic + * Run this test only using make tests TESTFILTER=GpsBlending + * + * @author Mathieu Bresciani + */ + +#include +#include + +#include "gps_blending.hpp" + +using matrix::Vector3f; + +class GpsBlendingTest : public ::testing::Test +{ +public: + sensor_gps_s getDefaultGpsData(); + void runSeconds(float duration_s, GpsBlending &gps_blending, sensor_gps_s &gps_data, int instance); + void runSeconds(float duration_s, GpsBlending &gps_blending, sensor_gps_s &gps_data0, sensor_gps_s &gps_data1); + + uint64_t _time_now_us{1000000}; +}; + +sensor_gps_s GpsBlendingTest::getDefaultGpsData() +{ + sensor_gps_s gps_data{}; + gps_data.timestamp = _time_now_us - 10e3; + gps_data.time_utc_usec = 0; + gps_data.lat = 47e7; + gps_data.lon = 9e7; + gps_data.alt = 800e3; + gps_data.alt_ellipsoid = 800e3; + gps_data.s_variance_m_s = 0.2f; + gps_data.c_variance_rad = 0.5f; + gps_data.eph = 0.7f; + gps_data.epv = 1.2f; + gps_data.hdop = 1.f; + gps_data.vdop = 1.f; + gps_data.noise_per_ms = 20; + gps_data.jamming_indicator = 40; + gps_data.vel_m_s = 1.f; + gps_data.vel_n_m_s = 1.f; + gps_data.vel_e_m_s = 1.f; + gps_data.vel_d_m_s = 1.f; + gps_data.cog_rad = 0.f; + gps_data.timestamp_time_relative = 0; + gps_data.heading = NAN; + gps_data.heading_offset = 0.f; + gps_data.fix_type = 4; + gps_data.vel_ned_valid = true; + gps_data.satellites_used = 8; + + return gps_data; +} + +void GpsBlendingTest::runSeconds(float duration_s, GpsBlending &gps_blending, sensor_gps_s &gps_data, int instance) +{ + const float dt = 0.1; + const uint64_t dt_us = static_cast(dt * 1e6f); + + for (int k = 0; k < static_cast(duration_s / dt); k++) { + gps_blending.setGpsData(gps_data, instance); + gps_blending.update(_time_now_us); + + _time_now_us += dt_us; + gps_data.timestamp += dt_us; + } +} + +void GpsBlendingTest::runSeconds(float duration_s, GpsBlending &gps_blending, sensor_gps_s &gps_data0, + sensor_gps_s &gps_data1) +{ + const float dt = 0.1; + const uint64_t dt_us = static_cast(dt * 1e6f); + + for (int k = 0; k < static_cast(duration_s / dt); k++) { + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + + gps_blending.update(_time_now_us); + + _time_now_us += dt_us; + gps_data0.timestamp += dt_us; + gps_data1.timestamp += dt_us; + } +} + +TEST_F(GpsBlendingTest, noData) +{ + GpsBlending gps_blending; + + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); + + gps_blending.update(_time_now_us); + + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); +} + +TEST_F(GpsBlendingTest, singleReceiver) +{ + GpsBlending gps_blending; + + gps_blending.setPrimaryInstance(-1); + sensor_gps_s gps_data = getDefaultGpsData(); + + gps_blending.setGpsData(gps_data, 1); + gps_blending.update(_time_now_us); + + _time_now_us += 200e3; + gps_data.timestamp = _time_now_us - 10e3; + gps_blending.setGpsData(gps_data, 1); + gps_blending.update(_time_now_us); + + EXPECT_EQ(gps_blending.getSelectedGps(), 1); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + // BUT IF: a second update is called without data + gps_blending.update(_time_now_us); + + // THEN: no new data should be available + EXPECT_EQ(gps_blending.getSelectedGps(), 1); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); + EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); +} + +TEST_F(GpsBlendingTest, dualReceiverNoBlending) +{ + GpsBlending gps_blending; + + // GIVEN: two receivers with the same prioity + gps_blending.setPrimaryInstance(-1); + sensor_gps_s gps_data0 = getDefaultGpsData(); + sensor_gps_s gps_data1 = getDefaultGpsData(); + + gps_data1.satellites_used = gps_data0.satellites_used + 2; // gps1 has more satellites than gps0 + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + gps_blending.update(_time_now_us); + + // THEN: gps1 should be selected because it has more satellites + EXPECT_EQ(gps_blending.getSelectedGps(), 1); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + gps_data1.satellites_used = gps_data0.satellites_used - 2; // gps1 has less satellites than gps0 + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + gps_blending.update(_time_now_us); + + // THEN: gps0 should be selected because it has more satellites + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); +} + +TEST_F(GpsBlendingTest, dualReceiverBlendingHPos) +{ + GpsBlending gps_blending; + + sensor_gps_s gps_data0 = getDefaultGpsData(); + sensor_gps_s gps_data1 = getDefaultGpsData(); + + gps_blending.setBlendingUseHPosAccuracy(true); + + gps_data1.eph = gps_data0.eph / 2.f; + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + gps_blending.update(_time_now_us); + + // THEN: the blended instance should be selected (2) + // and the eph should be adjusted + EXPECT_EQ(gps_blending.getSelectedGps(), 2); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + EXPECT_LT(gps_blending.getOutputGpsData().eph, gps_data0.eph); + EXPECT_FLOAT_EQ(gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than +} + +TEST_F(GpsBlendingTest, dualReceiverFailover) +{ + GpsBlending gps_blending; + + // GIVEN: a dual GPS setup with the first instance (0) + // set as primary + gps_blending.setPrimaryInstance(0); + gps_blending.setBlendingUseSpeedAccuracy(false); + gps_blending.setBlendingUseHPosAccuracy(false); + gps_blending.setBlendingUseVPosAccuracy(false); + + // WHEN: only the secondary receiver is available + sensor_gps_s gps_data1 = getDefaultGpsData(); + + const float duration_s = 10.f; + runSeconds(duration_s, gps_blending, gps_data1, 1); + + // THEN: the primary instance should be selected even if + // not available. No data is then available + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); + EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); + + // BUT WHEN: the data of the primary receiver is avaialbe + sensor_gps_s gps_data0 = getDefaultGpsData(); + gps_blending.setGpsData(gps_data0, 0); + gps_blending.update(_time_now_us); + + // THEN: the primary instance is selected and the data + // is available + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + runSeconds(duration_s, gps_blending, gps_data0, gps_data1); + + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + // BUT WHEN: the primary receiver isn't available anymore + runSeconds(duration_s, gps_blending, gps_data1, 1); + + // THEN: the data of the secondary receiver can be used + EXPECT_EQ(gps_blending.getSelectedGps(), 1); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + // AND IF: the primary receiver is available again and has + // better metrics than the secondary one + gps_data0.timestamp = gps_data1.timestamp; + gps_data0.satellites_used = gps_data1.satellites_used + 2; + + runSeconds(1.f, gps_blending, gps_data0, gps_data1); + + // THEN: the primary receiver should be used again + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); +} diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/params.c b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/params.c new file mode 100644 index 0000000..0136ad5 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_gps_position/params.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Multi GPS Blending Control Mask. + * + * Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance. + * 0 : Set to true to use speed accuracy + * 1 : Set to true to use horizontal position accuracy + * 2 : Set to true to use vertical position accuracy + * + * @group Sensors + * @min 0 + * @max 7 + * @bit 0 use speed accuracy + * @bit 1 use hpos accuracy + * @bit 2 use vpos accuracy + */ +PARAM_DEFINE_INT32(SENS_GPS_MASK, 0); + +/** + * Multi GPS Blending Time Constant + * + * Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences. + * + * + * @group Sensors + * @min 1.0 + * @max 100.0 + * @unit s + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f); + +/** + * Multi GPS primary instance + * + * When no blending is active, this defines the preferred GPS receiver instance. + * The GPS selection logic waits until the primary receiver is available to + * send data to the EKF even if a secondary instance is already available. + * The secondary instance is then only used if the primary one times out. + * + * To have an equal priority of all the instances, set this parameter to -1 and + * the best receiver will be used. + * + * This parameter has no effect if blending is active. + * + * @group Sensors + * @min -1 + * @max 1 + */ +PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0); diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/vehicle_imu/CMakeLists.txt new file mode 100644 index 0000000..8176327 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2020-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. +# +############################################################################ + +px4_add_library(vehicle_imu + Integrator.hpp + VehicleIMU.cpp + VehicleIMU.hpp +) +target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration) diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/prometheus_px4/src/modules/sensors/vehicle_imu/Integrator.hpp new file mode 100644 index 0000000..646e305 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_imu/Integrator.hpp @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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 Integrator.hpp + * + * A resettable integrator + * + * @author Lorenz Meier + * @author Julian Oes + */ + +#pragma once + +#include +#include + +class Integrator +{ +public: + Integrator() = default; + ~Integrator() = default; + + static constexpr float DT_MIN{FLT_MIN}; + static constexpr float DT_MAX{static_cast(UINT16_MAX) * 1e-6f}; + + /** + * Put an item into the integral. + * + * @param timestamp Timestamp of the current value. + * @param val Item to put. + * @return true if data was accepted and integrated. + */ + inline void put(const matrix::Vector3f &val, const float dt) + { + if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) { + _alpha += integrate(val, dt); + + } else { + reset(); + _last_val = val; + } + } + + /** + * Set reset interval during runtime. This won't reset the integrator. + * + * @param reset_interval New reset time interval for the integrator in microseconds. + */ + void set_reset_interval(uint32_t reset_interval_us) { _reset_interval_min = reset_interval_us * 1e-6f; } + + /** + * Set required samples for reset. This won't reset the integrator. + * + * @param reset_samples New reset time interval for the integrator. + */ + void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; } + uint8_t get_reset_samples() const { return _reset_samples_min; } + + /** + * Is the Integrator ready to reset? + * + * @return true if integrator has sufficient data (minimum interval & samples satisfied) to reset. + */ + inline bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); } + + void reset() + { + _alpha.zero(); + _integral_dt = 0; + _integrated_samples = 0; + } + + /* Reset integrator and return current integral & integration time + * + * @param integral_dt Get the dt in us of the current integration. + * @return true if integral valid + */ + bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) + { + if (integral_ready()) { + integral = _alpha; + integral_dt = roundf(_integral_dt * 1e6f); // seconds to microseconds + + reset(); + + return true; + } + + return false; + } + +protected: + + inline matrix::Vector3f integrate(const matrix::Vector3f &val, const float dt) + { + // Use trapezoidal integration to calculate the delta integral + _integrated_samples++; + _integral_dt += dt; + const matrix::Vector3f delta_alpha{(val + _last_val) *dt * 0.5f}; + _last_val = val; + + return delta_alpha; + } + + matrix::Vector3f _alpha{0.f, 0.f, 0.f}; /**< integrated value before coning corrections are applied */ + matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */ + float _integral_dt{0}; + + float _reset_interval_min{0.005f}; /**< the interval after which the content will be published and the integrator reset */ + uint8_t _reset_samples_min{1}; + + uint8_t _integrated_samples{0}; +}; + +class IntegratorConing : public Integrator +{ +public: + IntegratorConing() = default; + ~IntegratorConing() = default; + + /** + * Put an item into the integral. + * + * @param timestamp Timestamp of the current value. + * @param val Item to put. + * @return true if data was accepted and integrated. + */ + inline void put(const matrix::Vector3f &val, const float dt) + { + if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) { + // Use trapezoidal integration to calculate the delta integral + const matrix::Vector3f delta_alpha{integrate(val, dt)}; + + // Calculate coning corrections + // Coning compensation derived by Paul Riseborough and Jonathan Challinger, + // following: + // Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms + // Sourced: https://arc.aiaa.org/doi/pdf/10.2514/2.4228 + // Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m + _beta += ((_last_alpha + _last_delta_alpha * (1.f / 6.f)) % delta_alpha) * 0.5f; + _last_delta_alpha = delta_alpha; + _last_alpha = _alpha; + + // accumulate delta integrals + _alpha += delta_alpha; + + } else { + reset(); + _last_val = val; + } + } + + void reset() + { + Integrator::reset(); + _beta.zero(); + _last_alpha.zero(); + } + + /* Reset integrator and return current integral & integration time + * + * @param integral_dt Get the dt in us of the current integration. + * @return true if integral valid + */ + bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) + { + if (Integrator::reset(integral, integral_dt)) { + // apply coning corrections + integral += _beta; + _beta.zero(); + _last_alpha.zero(); + return true; + } + + return false; + } + +private: + matrix::Vector3f _beta{0.f, 0.f, 0.f}; /**< accumulated coning corrections */ + matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */ + matrix::Vector3f _last_alpha{0.f, 0.f, 0.f}; /**< previous value of _alpha */ + +}; diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/prometheus_px4/src/modules/sensors/vehicle_imu/VehicleIMU.cpp new file mode 100644 index 0000000..fb31014 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -0,0 +1,578 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "VehicleIMU.hpp" + +#include +#include + +#include + +using namespace matrix; + +using math::constrain; + +namespace sensors +{ + +VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, config), + _sensor_accel_sub(ORB_ID(sensor_accel), accel_index), + _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index), + _instance(instance) +{ + _imu_integration_interval_us = 1e6f / _param_imu_integ_rate.get(); + + _accel_integrator.set_reset_interval(_imu_integration_interval_us); + _accel_integrator.set_reset_samples(sensor_accel_s::ORB_QUEUE_LENGTH); + + _gyro_integrator.set_reset_interval(_imu_integration_interval_us); + _gyro_integrator.set_reset_samples(sensor_gyro_s::ORB_QUEUE_LENGTH); + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + // currently with lockstep every raw sample needs a corresponding vehicle_imu publication + _sensor_gyro_sub.set_required_updates(1); +#else + // schedule conservatively until the actual accel & gyro rates are known + _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); +#endif + + // advertise immediately to ensure consistent ordering + _vehicle_imu_pub.advertise(); + _vehicle_imu_status_pub.advertise(); +} + +VehicleIMU::~VehicleIMU() +{ + Stop(); + + perf_free(_accel_generation_gap_perf); + perf_free(_gyro_generation_gap_perf); + + _vehicle_imu_pub.unadvertise(); + _vehicle_imu_status_pub.unadvertise(); +} + +bool VehicleIMU::Start() +{ + // force initial updates + ParametersUpdate(true); + + _sensor_gyro_sub.registerCallback(); + ScheduleNow(); + return true; +} + +void VehicleIMU::Stop() +{ + // clear all registered callbacks + _sensor_gyro_sub.unregisterCallback(); + + Deinit(); +} + +void VehicleIMU::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + const auto imu_integ_rate_prev = _param_imu_integ_rate.get(); + + updateParams(); + + _accel_calibration.ParametersUpdate(); + _gyro_calibration.ParametersUpdate(); + + // constrain IMU integration time 1-10 milliseconds (100-1000 Hz) + int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), + (int32_t)100, math::max(_param_imu_gyro_ratemax.get(), (int32_t) 1000)); + + if (imu_integration_rate_hz != _param_imu_integ_rate.get()) { + PX4_WARN("IMU_INTEG_RATE updated %" PRId32 " -> %" PRIu32, _param_imu_integ_rate.get(), imu_integration_rate_hz); + _param_imu_integ_rate.set(imu_integration_rate_hz); + _param_imu_integ_rate.commit_no_notification(); + } + + _imu_integration_interval_us = 1e6f / imu_integration_rate_hz; + + if (_param_imu_integ_rate.get() != imu_integ_rate_prev) { + // force update + _update_integrator_config = true; + } + } +} + +void VehicleIMU::Run() +{ + const hrt_abstime now_us = hrt_absolute_time(); + + // backup schedule + ScheduleDelayed(_backup_schedule_timeout_us); + + ParametersUpdate(); + + if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) { + return; + } + + // reset data gap monitor + _data_gap = false; + + while (_sensor_gyro_sub.updated() || _sensor_accel_sub.updated()) { + bool updated = false; + + bool consume_all_gyro = !_intervals_configured || _data_gap; + + // monitor scheduling latency and force catch up with latest gyro if falling behind + if (_sensor_gyro_sub.updated() && (_gyro_update_latency_mean.count() > 100) + && (_gyro_update_latency_mean.mean()(1) > _gyro_interval_us * 1e-6f)) { + + PX4_DEBUG("gyro update mean sample latency: %.6f, publish latency %.6f", + (double)_gyro_update_latency_mean.mean()(0), + (double)_gyro_update_latency_mean.mean()(1)); + + consume_all_gyro = true; + } + + // update gyro until integrator ready and not falling behind + if (!_gyro_integrator.integral_ready() || consume_all_gyro) { + if (UpdateGyro()) { + updated = true; + } + } + + + // update accel until integrator ready and caught up to gyro + while (_sensor_accel_sub.updated() + && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap + || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)))) { + + if (UpdateAccel()) { + updated = true; + } + } + + // reconfigure integrators if calculated sensor intervals have changed + if (_update_integrator_config || !_intervals_configured) { + UpdateIntegratorConfiguration(); + } + + // check for additional updates and that we're fully caught up before publishing + if ((consume_all_gyro || _data_gap) && _sensor_gyro_sub.updated()) { + continue; + } + + // publish if both accel & gyro integrators are ready + if (_intervals_configured && _accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { + if (Publish()) { + // record gyro publication latency and integrated samples + if (_gyro_update_latency_mean.count() > 10000) { + // reset periodically to avoid numerical issues + _gyro_update_latency_mean.reset(); + } + + const float time_run_s = now_us * 1e-6f; + + _gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f}); + + return; + } + } + + // finish if there are no more updates, but didn't publish + if (!updated) { + return; + } + } +} + +bool VehicleIMU::UpdateAccel() +{ + bool updated = false; + + // integrate queued accel + sensor_accel_s accel; + + if (_sensor_accel_sub.update(&accel)) { + if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) { + _data_gap = true; + perf_count(_accel_generation_gap_perf); + + // reset average sample measurement + _accel_interval_mean.reset(); + + } else { + // collect sample interval average for filters + if (_accel_timestamp_sample_last != 0) { + float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last; + _accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples}); + } + + if (_accel_interval_mean.valid() && (_accel_interval_mean.count() > 100 || !PX4_ISFINITE(_accel_interval_best_variance)) + && ((_accel_interval_mean.variance()(0) < _accel_interval_best_variance) || (_accel_interval_mean.count() > 1000))) { + // update sample rate if previously invalid or changed + const float interval_delta_us = fabsf(_accel_interval_mean.mean()(0) - _accel_interval_us); + const float percent_changed = interval_delta_us / _accel_interval_us; + + if (!PX4_ISFINITE(_accel_interval_us) || (percent_changed > 0.001f)) { + // update integrator configuration if interval has changed by more than 10% + if (interval_delta_us > 0.1f * _accel_interval_us) { + _update_integrator_config = true; + } + + _accel_interval_us = _accel_interval_mean.mean()(0); + _accel_interval_best_variance = _accel_interval_mean.variance()(0); + + _status.accel_rate_hz = 1e6f / _accel_interval_mean.mean()(0); + _status.accel_raw_rate_hz = 1e6f / _accel_interval_mean.mean()(1); // FIFO + _publish_status = true; + } + + if (_accel_interval_mean.count() > 10000) { + // reset periodically to prevent numerical issues + _accel_interval_mean.reset(); + } + } + } + + _accel_last_generation = _sensor_accel_sub.get_last_generation(); + + _accel_calibration.set_device_id(accel.device_id); + + if (accel.error_count != _status.accel_error_count) { + _publish_status = true; + _status.accel_error_count = accel.error_count; + } + + const Vector3f accel_raw{accel.x, accel.y, accel.z}; + _accel_sum += accel_raw; + _accel_temperature += accel.temperature; + _accel_sum_count++; + + const float dt = (accel.timestamp_sample - _accel_timestamp_sample_last) * 1e-6f; + _accel_timestamp_sample_last = accel.timestamp_sample; + + _accel_integrator.put(accel_raw, dt); + updated = true; + + if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { + // rotate sensor clip counts into vehicle body frame + const Vector3f clipping{_accel_calibration.rotation() * + Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; + + // round to get reasonble clip counts per axis (after board rotation) + const uint8_t clip_x = roundf(fabsf(clipping(0))); + const uint8_t clip_y = roundf(fabsf(clipping(1))); + const uint8_t clip_z = roundf(fabsf(clipping(2))); + + _status.accel_clipping[0] += clip_x; + _status.accel_clipping[1] += clip_y; + _status.accel_clipping[2] += clip_z; + + if (clip_x > 0) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X; + } + + if (clip_y > 0) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y; + } + + if (clip_z > 0) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z; + } + + _publish_status = true; + + if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) { + // start notifying the user periodically if there's significant continuous clipping + const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2]; + + if (clipping_total > _last_clipping_notify_total_count + 1000) { + mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!", _instance); + _last_clipping_notify_time = accel.timestamp_sample; + _last_clipping_notify_total_count = clipping_total; + } + } + } + } + + return updated; +} + +bool VehicleIMU::UpdateGyro() +{ + bool updated = false; + + // integrate queued gyro + sensor_gyro_s gyro; + + if (_sensor_gyro_sub.update(&gyro)) { + if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { + _data_gap = true; + perf_count(_gyro_generation_gap_perf); + + // reset average sample measurement + _gyro_interval_mean.reset(); + + } else { + // collect sample interval average for filters + if (_gyro_timestamp_sample_last != 0) { + float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last; + _gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples}); + } + + if (_gyro_interval_mean.valid() && (_gyro_interval_mean.count() > 100 || !PX4_ISFINITE(_gyro_interval_best_variance)) + && ((_gyro_interval_mean.variance()(0) < _gyro_interval_best_variance) || (_gyro_interval_mean.count() > 1000))) { + // update sample rate if previously invalid or changed + const float interval_delta_us = fabsf(_gyro_interval_mean.mean()(0) - _gyro_interval_us); + const float percent_changed = interval_delta_us / _gyro_interval_us; + + if (!PX4_ISFINITE(_gyro_interval_us) || (percent_changed > 0.001f)) { + // update integrator configuration if interval has changed by more than 10% + if (interval_delta_us > 0.1f * _gyro_interval_us) { + _update_integrator_config = true; + } + + _gyro_interval_us = _gyro_interval_mean.mean()(0); + _gyro_interval_best_variance = _gyro_interval_mean.variance()(0); + + _status.gyro_rate_hz = 1e6f / _gyro_interval_mean.mean()(0); + _status.gyro_raw_rate_hz = 1e6f / _gyro_interval_mean.mean()(1); // FIFO + _publish_status = true; + } + + if (_gyro_interval_mean.count() > 10000) { + // reset periodically to prevent numerical issues + _gyro_interval_mean.reset(); + } + } + } + + _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); + _gyro_timestamp_last = gyro.timestamp; + + _gyro_calibration.set_device_id(gyro.device_id); + + if (gyro.error_count != _status.gyro_error_count) { + _publish_status = true; + _status.gyro_error_count = gyro.error_count; + } + + const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z}; + _gyro_sum += gyro_raw; + _gyro_temperature += gyro.temperature; + _gyro_sum_count++; + + const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; + _gyro_timestamp_sample_last = gyro.timestamp_sample; + + _gyro_integrator.put(gyro_raw, dt); + updated = true; + } + + return updated; +} + +bool VehicleIMU::Publish() +{ + bool updated = false; + + vehicle_imu_s imu; + Vector3f delta_angle; + Vector3f delta_velocity; + + if (_accel_integrator.reset(delta_velocity, imu.delta_velocity_dt) + && _gyro_integrator.reset(delta_angle, imu.delta_angle_dt)) { + + if (_accel_calibration.enabled() && _gyro_calibration.enabled()) { + + // delta angle: apply offsets, scale, and board rotation + _gyro_calibration.SensorCorrectionsUpdate(); + const float gyro_dt_inv = 1.e6f / imu.delta_angle_dt; + const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv}; + + // delta velocity: apply offsets, scale, and board rotation + _accel_calibration.SensorCorrectionsUpdate(); + const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt; + Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv}; + + UpdateAccelVibrationMetrics(delta_velocity_corrected); + UpdateGyroVibrationMetrics(delta_angle_corrected); + + // vehicle_imu_status + // publish before vehicle_imu so that error counts are available synchronously if needed + if ((_accel_sum_count > 0) && (_gyro_sum_count > 0) + && (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) { + + _status.accel_device_id = _accel_calibration.device_id(); + _status.gyro_device_id = _gyro_calibration.device_id(); + + // mean accel + const Vector3f accel_mean{_accel_calibration.Correct(_accel_sum / _accel_sum_count)}; + accel_mean.copyTo(_status.mean_accel); + _status.temperature_accel = _accel_temperature / _accel_sum_count; + _accel_sum.zero(); + _accel_temperature = 0; + _accel_sum_count = 0; + + // mean gyro + const Vector3f gyro_mean{_gyro_calibration.Correct(_gyro_sum / _gyro_sum_count)}; + gyro_mean.copyTo(_status.mean_gyro); + _status.temperature_gyro = _gyro_temperature / _gyro_sum_count; + _gyro_sum.zero(); + _gyro_temperature = 0; + _gyro_sum_count = 0; + + _status.timestamp = hrt_absolute_time(); + _vehicle_imu_status_pub.publish(_status); + + _publish_status = false; + } + + // publish vehicle_imu + imu.timestamp_sample = _gyro_timestamp_sample_last; + imu.accel_device_id = _accel_calibration.device_id(); + imu.gyro_device_id = _gyro_calibration.device_id(); + delta_angle_corrected.copyTo(imu.delta_angle); + delta_velocity_corrected.copyTo(imu.delta_velocity); + imu.delta_velocity_clipping = _delta_velocity_clipping; + imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count(); + imu.timestamp = hrt_absolute_time(); + _vehicle_imu_pub.publish(imu); + + // reset clip counts + _delta_velocity_clipping = 0; + + updated = true; + } + } + + return updated; +} + +void VehicleIMU::UpdateIntegratorConfiguration() +{ + if (PX4_ISFINITE(_accel_interval_us) && PX4_ISFINITE(_gyro_interval_us)) { + + // determine number of sensor samples that will get closest to the desired integration interval + uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us)); + + // if gyro samples exceeds queue depth, instead round to nearest even integer to improve scheduling options + if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { + gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us / 2) * 2); + } + + uint32_t integration_interval_us = roundf(gyro_integral_samples * _gyro_interval_us); + + // accel follows gyro as closely as possible + uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / _accel_interval_us)); + + // let the gyro set the configuration and scheduling + // relaxed minimum integration time required + _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval_us)); + _accel_integrator.set_reset_samples(accel_integral_samples); + + _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us)); + _gyro_integrator.set_reset_samples(gyro_integral_samples); + + _backup_schedule_timeout_us = math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us, + sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us); + + // gyro: find largest integer multiple of gyro_integral_samples + for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) { + if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { + gyro_integral_samples /= 2; + } + + if (gyro_integral_samples % n == 0) { + _sensor_gyro_sub.set_required_updates(n); + + _intervals_configured = true; + _update_integrator_config = false; + + PX4_DEBUG("accel (%" PRIu32 "), gyro (%" PRIu32 "), accel samples: %" PRIu8 ", gyro samples: %" PRIu8 + ", accel interval: %.1f, gyro interval: %.1f sub samples: %d", + _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples, + (double)_accel_interval_us, (double)_gyro_interval_us, n); + + break; + } + } + } +} + +void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &delta_velocity) +{ + // Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) + const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev; + _status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric + 0.01f * delta_velocity_diff.norm(); + + _delta_velocity_prev = delta_velocity; +} + +void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle) +{ + // Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) + const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev; + _status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric + 0.01f * delta_angle_diff.norm(); + + // Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) + const Vector3f coning_metric = delta_angle % _delta_angle_prev; + _status.gyro_coning_vibration = 0.99f * _status.gyro_coning_vibration + 0.01f * coning_metric.norm(); + + _delta_angle_prev = delta_angle; +} + +void VehicleIMU::PrintStatus() +{ + PX4_INFO("%" PRIu8 " - Accel ID: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro ID: %" PRIu32 + ", interval: %.1f us (SD %.1f us)", + _instance, _accel_calibration.device_id(), (double)_accel_interval_us, (double)sqrtf(_accel_interval_best_variance), + _gyro_calibration.device_id(), (double)_gyro_interval_us, (double)sqrtf(_gyro_interval_best_variance)); + + PX4_DEBUG("gyro update mean sample latency: %.6f s, publish latency %.6f s, gyro interval %.6f s", + (double)_gyro_update_latency_mean.mean()(0), (double)_gyro_update_latency_mean.mean()(1), + (double)(_gyro_interval_us * 1e-6f)); + + perf_print_counter(_accel_generation_gap_perf); + perf_print_counter(_gyro_generation_gap_perf); + + _accel_calibration.PrintStatus(); + _gyro_calibration.PrintStatus(); +} + +} // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/prometheus_px4/src/modules/sensors/vehicle_imu/VehicleIMU.hpp new file mode 100644 index 0000000..4ea00d3 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "Integrator.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ + +class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + VehicleIMU() = delete; + VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config); + + ~VehicleIMU() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void ParametersUpdate(bool force = false); + bool Publish(); + void Run() override; + + bool UpdateAccel(); + bool UpdateGyro(); + + void UpdateIntegratorConfiguration(); + void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); + void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); + + uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; + uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _sensor_accel_sub; + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub; + + calibration::Accelerometer _accel_calibration{}; + calibration::Gyroscope _gyro_calibration{}; + + Integrator _accel_integrator{}; + IntegratorConing _gyro_integrator{}; + + uint32_t _imu_integration_interval_us{5000}; + + hrt_abstime _accel_timestamp_sample_last{0}; + hrt_abstime _gyro_timestamp_sample_last{0}; + hrt_abstime _gyro_timestamp_last{0}; + + math::WelfordMean _accel_interval_mean{}; + math::WelfordMean _gyro_interval_mean{}; + + math::WelfordMean _gyro_update_latency_mean{}; + + float _accel_interval_best_variance{INFINITY}; + float _gyro_interval_best_variance{INFINITY}; + + float _accel_interval_us{NAN}; + float _gyro_interval_us{NAN}; + + unsigned _accel_last_generation{0}; + unsigned _gyro_last_generation{0}; + + matrix::Vector3f _accel_sum{}; + matrix::Vector3f _gyro_sum{}; + int _accel_sum_count{0}; + int _gyro_sum_count{0}; + float _accel_temperature{0}; + float _gyro_temperature{0}; + + matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement + matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement + + vehicle_imu_status_s _status{}; + + uint8_t _delta_velocity_clipping{0}; + + hrt_abstime _last_clipping_notify_time{0}; + uint64_t _last_clipping_notify_total_count{0}; + orb_advert_t _mavlink_log_pub{nullptr}; + + uint32_t _backup_schedule_timeout_us{20000}; + + bool _data_gap{false}; + bool _update_integrator_config{true}; + bool _intervals_configured{false}; + bool _publish_status{false}; + + const uint8_t _instance; + + perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; + perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_imu_integ_rate, + (ParamInt) _param_imu_gyro_ratemax + ) +}; + +} // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_imu/imu_parameters.c b/src/prometheus_px4/src/modules/sensors/vehicle_imu/imu_parameters.c new file mode 100644 index 0000000..3077e14 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_imu/imu_parameters.c @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** +* IMU integration rate. +* +* The rate at which raw IMU data is integrated to produce delta angles and delta velocities. +* Recommended to set this to a multiple of the estimator update period (currently 10 ms for ekf2). +* +* @min 100 +* @max 1000 +* @value 100 100 Hz +* @value 200 200 Hz +* @value 250 250 Hz +* @value 400 400 Hz +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_INTEG_RATE, 200); diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt b/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt new file mode 100644 index 0000000..a864b95 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(vehicle_magnetometer + VehicleMagnetometer.cpp + VehicleMagnetometer.hpp +) + +target_link_libraries(vehicle_magnetometer + PRIVATE + px4_work_queue + sensor_calibration +) diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp new file mode 100644 index 0000000..9ce0a13 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -0,0 +1,546 @@ +/**************************************************************************** + * + * Copyright (c) 2020, 2001 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 "VehicleMagnetometer.hpp" + +#include +#include + +namespace sensors +{ + +using namespace matrix; + +static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; + +VehicleMagnetometer::VehicleMagnetometer() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + param_find("CAL_MAG_SIDES"); + param_find("CAL_MAG_ROT_AUTO"); + + _voter.set_timeout(SENSOR_TIMEOUT); + _voter.set_equal_value_threshold(1000); + + ParametersUpdate(true); +} + +VehicleMagnetometer::~VehicleMagnetometer() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleMagnetometer::Start() +{ + ScheduleNow(); + return true; +} + +void VehicleMagnetometer::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } +} + +void VehicleMagnetometer::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + + // Mag compensation type + MagCompensationType mag_comp_typ = static_cast(_param_mag_comp_typ.get()); + + if (mag_comp_typ != _mag_comp_type) { + // check mag power compensation type (change battery current subscription instance if necessary) + switch (mag_comp_typ) { + case MagCompensationType::Current_inst0: + _battery_status_sub.ChangeInstance(0); + break; + + case MagCompensationType::Current_inst1: + _battery_status_sub.ChangeInstance(1); + break; + + case MagCompensationType::Throttle: + break; + + default: + + // ensure power compensation is disabled + for (auto &cal : _calibration) { + cal.UpdatePower(0.f); + } + + break; + } + } + + _mag_comp_type = mag_comp_typ; + + // update mag priority (CAL_MAGx_PRIO) + for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) { + const int32_t priority_old = _calibration[mag].priority(); + _calibration[mag].ParametersUpdate(); + const int32_t priority_new = _calibration[mag].priority(); + + if (priority_old != priority_new) { + if (_priority[mag] == priority_old) { + _priority[mag] = priority_new; + + } else { + // change relative priority to incorporate any sensor faults + int priority_change = priority_new - priority_old; + _priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100); + } + } + } + } +} + +void VehicleMagnetometer::MagCalibrationUpdate() +{ + // State variance assumed for magnetometer bias storage. + // This is a reference variance used to calculate the fraction of learned magnetometer bias that will be used to update the stored value. + // Larger values cause a larger fraction of the learned biases to be used. + static constexpr float magb_vref = 2.5e-7f; + static constexpr float min_var_allowed = magb_vref * 0.01f; + static constexpr float max_var_allowed = magb_vref * 100.f; + + if (_armed) { + static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]); + + for (int i = 0; i < math::min(_estimator_sensor_bias_subs.size(), mag_cal_size); i++) { + estimator_sensor_bias_s estimator_sensor_bias; + + if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) { + + const Vector3f bias{estimator_sensor_bias.mag_bias}; + const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance}; + + const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) + && (estimator_sensor_bias.mag_device_id != 0) && estimator_sensor_bias.mag_bias_valid + && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); + + if (valid) { + // find corresponding mag calibration + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) { + + const auto old_offset = _mag_cal[i].mag_offset; + + _mag_cal[i].device_id = estimator_sensor_bias.mag_device_id; + _mag_cal[i].mag_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias); + _mag_cal[i].mag_bias_variance = bias_variance; + + _mag_cal_available = true; + + if ((old_offset - _mag_cal[i].mag_offset).longerThan(0.01f)) { + PX4_DEBUG("Mag %d (%d) est. offset saved: [% 05.3f % 05.3f % 05.3f] (bias [% 05.3f % 05.3f % 05.3f])", + mag_index, _mag_cal[i].device_id, + (double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2), + (double)bias(0), (double)bias(1), (double)bias(2)); + } + + break; + } + } + } + } + } + + } else if (_mag_cal_available) { + // not armed and mag cal available + bool calibration_param_save_needed = false; + // iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme + Vector3f state_variance{magb_vref, magb_vref, magb_vref}; + + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + // apply all valid saved offsets + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) { + + const Vector3f mag_cal_orig{_calibration[mag_index].offset()}; + Vector3f mag_cal_offset{_calibration[mag_index].offset()}; + + // calculate weighting using ratio of variances and update stored bias values + const Vector3f &observation = _mag_cal[i].mag_offset; + const Vector3f &obs_variance = _mag_cal[i].mag_bias_variance; + + for (int axis_index = 0; axis_index < 3; axis_index++) { + const float innovation_variance = state_variance(axis_index) + obs_variance(axis_index); + const float innovation = mag_cal_offset(axis_index) - observation(axis_index); + const float kalman_gain = state_variance(axis_index) / innovation_variance; + mag_cal_offset(axis_index) -= innovation * kalman_gain; + state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f); + } + + if (_calibration[mag_index].set_offset(mag_cal_offset)) { + + PX4_INFO("%d (%" PRIu32 ") EST:%d offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])", + mag_index, _calibration[mag_index].device_id(), i, + (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), + (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), + (double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2)); + + calibration_param_save_needed = true; + } + + // clear + _mag_cal[i].device_id = 0; + _mag_cal[i].mag_offset.zero(); + _mag_cal[i].mag_bias_variance.zero(); + } + } + } + + if (calibration_param_save_needed) { + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].device_id() != 0) { + _calibration[mag_index].ParametersSave(); + } + } + + _mag_cal_available = false; + } + } +} + +void VehicleMagnetometer::Run() +{ + perf_begin(_cycle_perf); + + ParametersUpdate(); + + // check vehicle status for changes to armed state + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { + _armed = vehicle_control_mode.flag_armed; + } + } + + if (_mag_comp_type != MagCompensationType::Disabled) { + // update power signal for mag compensation + if (_armed && (_mag_comp_type == MagCompensationType::Throttle)) { + actuator_controls_s controls; + + if (_actuator_controls_0_sub.update(&controls)) { + for (auto &cal : _calibration) { + cal.UpdatePower(controls.control[actuator_controls_s::INDEX_THROTTLE]); + } + } + + } else if ((_mag_comp_type == MagCompensationType::Current_inst0) + || (_mag_comp_type == MagCompensationType::Current_inst1)) { + + battery_status_s bat_stat; + + if (_battery_status_sub.update(&bat_stat)) { + float power = bat_stat.current_a * 0.001f; // current in [kA] + + for (auto &cal : _calibration) { + cal.UpdatePower(power); + } + } + + } else { + for (auto &cal : _calibration) { + cal.UpdatePower(0.f); + } + } + } + + bool updated[MAX_SENSOR_COUNT] {}; + + for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { + + if (!_calibration[uorb_index].enabled()) { + continue; + } + + if (!_advertised[uorb_index]) { + // use data's timestamp to throttle advertisement checks + if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) { + if (_sensor_sub[uorb_index].advertised()) { + if (uorb_index > 0) { + /* the first always exists, but for each further sensor, add a new validator */ + if (!_voter.add_new_validator()) { + PX4_ERR("failed to add validator for %s %i", "MAG", uorb_index); + } + } + + _advertised[uorb_index] = true; + + // advertise outputs in order if publishing all + if (!_param_sens_mag_mode.get()) { + for (int instance = 0; instance < uorb_index; instance++) { + _vehicle_magnetometer_pub[instance].advertise(); + } + } + + if (_selected_sensor_sub_index < 0) { + _sensor_sub[uorb_index].registerCallback(); + } + + } else { + _last_data[uorb_index].timestamp = hrt_absolute_time(); + } + } + + } + + if (_advertised[uorb_index]) { + sensor_mag_s report; + + while (_sensor_sub[uorb_index].update(&report)) { + updated[uorb_index] = true; + + if (_calibration[uorb_index].device_id() != report.device_id) { + _calibration[uorb_index].set_device_id(report.device_id, report.is_external); + _priority[uorb_index] = _calibration[uorb_index].priority(); + } + + if (_calibration[uorb_index].enabled()) { + const Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z}); + + float mag_array[3] {vect(0), vect(1), vect(2)}; + _voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]); + + _timestamp_sample_sum[uorb_index] += report.timestamp_sample; + _mag_sum[uorb_index] += vect; + _mag_sum_count[uorb_index]++; + + _last_data[uorb_index].timestamp_sample = report.timestamp_sample; + _last_data[uorb_index].device_id = report.device_id; + _last_data[uorb_index].x = vect(0); + _last_data[uorb_index].y = vect(1); + _last_data[uorb_index].z = vect(2); + } + } + } + } + + // check for the current best sensor + int best_index = 0; + _voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + if (_selected_sensor_sub_index != best_index) { + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } + + if (_param_sens_mag_mode.get()) { + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("%s switch from #%" PRId8 " -> #%d", "MAG", _selected_sensor_sub_index, best_index); + } + } + + _selected_sensor_sub_index = best_index; + _sensor_sub[_selected_sensor_sub_index].registerCallback(); + } + } + + // Publish + if (_param_sens_mag_mode.get()) { + // publish only best mag + if ((_selected_sensor_sub_index >= 0) + && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR) + && updated[_selected_sensor_sub_index]) { + + Publish(_selected_sensor_sub_index); + } + + } else { + // publish all + for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { + // publish all magnetometers as separate instances + if (updated[uorb_index] && (_calibration[uorb_index].device_id() != 0)) { + Publish(uorb_index, true); + } + } + } + + + // check failover and report + if (_param_sens_mag_mode.get()) { + if (_last_failover_count != _voter.failover_count()) { + uint32_t flags = _voter.failover_state(); + int failover_index = _voter.failover_index(); + + if (flags != DataValidator::ERROR_FLAG_NO_ERROR) { + if (failover_index != -1) { + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_error_message > 3_s) { + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!", + "MAG", + failover_index, + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + _last_error_message = now; + } + + // reduce priority of failed sensor to the minimum + _priority[failover_index] = 1; + } + } + + _last_failover_count = _voter.failover_count(); + } + } + + if (!_armed) { + calcMagInconsistency(); + } + + MagCalibrationUpdate(); + + // reschedule timeout + ScheduleDelayed(20_ms); + + perf_end(_cycle_perf); +} + +void VehicleMagnetometer::Publish(uint8_t instance, bool multi) +{ + if ((_param_sens_mag_rate.get() > 0) && ((_last_publication_timestamp[instance] == 0) || + (hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())))) { + + const Vector3f magnetometer_data = _mag_sum[instance] / _mag_sum_count[instance]; + const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _mag_sum_count[instance]; + + // reset + _timestamp_sample_sum[instance] = 0; + _mag_sum[instance].zero(); + _mag_sum_count[instance] = 0; + + // populate vehicle_magnetometer with primary mag and publish + vehicle_magnetometer_s out{}; + out.timestamp_sample = timestamp_sample; + out.device_id = _calibration[instance].device_id(); + magnetometer_data.copyTo(out.magnetometer_ga); + out.calibration_count = _calibration[instance].calibration_count(); + + out.timestamp = hrt_absolute_time(); + + if (multi) { + _vehicle_magnetometer_pub[instance].publish(out); + + } else { + // otherwise only ever publish the first instance + _vehicle_magnetometer_pub[0].publish(out); + } + + _last_publication_timestamp[instance] = out.timestamp; + } +} + +void VehicleMagnetometer::calcMagInconsistency() +{ + sensor_preflight_mag_s preflt{}; + + const sensor_mag_s &primary_mag_report = _last_data[_selected_sensor_sub_index]; + const Vector3f primary_mag(primary_mag_report.x, primary_mag_report.y, + primary_mag_report.z); // primary mag field vector + + float mag_angle_diff_max = 0.0f; // the maximum angle difference + unsigned check_index = 0; // the number of sensors the primary has been checked against + + // Check each sensor against the primary + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + // check that the sensor we are checking against is not the same as the primary + if (_advertised[i] && (_priority[i] > 0) && (i != _selected_sensor_sub_index)) { + // calculate angle to 3D magnetic field vector of the primary sensor + const sensor_mag_s ¤t_mag_report = _last_data[i]; + Vector3f current_mag{current_mag_report.x, current_mag_report.y, current_mag_report.z}; + + float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle(); + + // complementary filter to not fail/pass on single outliers + _mag_angle_diff[check_index] *= 0.95f; + _mag_angle_diff[check_index] += 0.05f * angle_error; + + mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]); + + // increment the check index + check_index++; + } + + // check to see if the maximum number of checks has been reached and break + if (check_index >= 2) { + break; + } + } + + // get the vector length of the largest difference and write to the combined sensor struct + // will be zero if there is only one magnetometer and hence nothing to compare + preflt.mag_inconsistency_angle = mag_angle_diff_max; + + preflt.timestamp = hrt_absolute_time(); + _sensor_preflight_mag_pub.publish(preflt); +} + +void VehicleMagnetometer::PrintStatus() +{ + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("selected magnetometer: %" PRIu32 " (%" PRId8 ")", _last_data[_selected_sensor_sub_index].device_id, + _selected_sensor_sub_index); + } + + _voter.print(); + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (_advertised[i] && (_priority[i] > 0)) { + _calibration[i].PrintStatus(); + } + } +} + +}; // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp new file mode 100644 index 0000000..ca62cf8 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * 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 "data_validator/DataValidatorGroup.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ +class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + VehicleMagnetometer(); + ~VehicleMagnetometer() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(bool force = false); + + void Publish(uint8_t instance, bool multi = false); + + /** + * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers + */ + void calcMagInconsistency(); + void MagCalibrationUpdate(); + + static constexpr int MAX_SENSOR_COUNT = 4; + + uORB::Publication _sensor_preflight_mag_pub{ORB_ID(sensor_preflight_mag)}; + + uORB::PublicationMulti _vehicle_magnetometer_pub[MAX_SENSOR_COUNT] { + {ORB_ID(vehicle_magnetometer)}, + {ORB_ID(vehicle_magnetometer)}, + {ORB_ID(vehicle_magnetometer)}, + {ORB_ID(vehicle_magnetometer)}, + }; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + + // Used to check, save and use learned magnetometer biases + uORB::SubscriptionMultiArray _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias}; + + bool _mag_cal_available{false}; + + struct MagCal { + uint32_t device_id{0}; + matrix::Vector3f mag_offset{}; + matrix::Vector3f mag_bias_variance{}; + } _mag_cal[ORB_MULTI_MAX_INSTANCES] {}; + + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { + {this, ORB_ID(sensor_mag), 0}, + {this, ORB_ID(sensor_mag), 1}, + {this, ORB_ID(sensor_mag), 2}, + {this, ORB_ID(sensor_mag), 3} + }; + + calibration::Magnetometer _calibration[MAX_SENSOR_COUNT]; + + // Magnetometer interference compensation + enum class MagCompensationType { + Disabled = 0, + Throttle, + Current_inst0, + Current_inst1 + }; + MagCompensationType _mag_comp_type{MagCompensationType::Disabled}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + hrt_abstime _last_error_message{0}; + orb_advert_t _mavlink_log_pub{nullptr}; + + DataValidatorGroup _voter{1}; + unsigned _last_failover_count{0}; + + uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0}; + matrix::Vector3f _mag_sum[MAX_SENSOR_COUNT] {}; + int _mag_sum_count[MAX_SENSOR_COUNT] {}; + hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {}; + + sensor_mag_s _last_data[MAX_SENSOR_COUNT] {}; + bool _advertised[MAX_SENSOR_COUNT] {}; + + float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */ + + uint8_t _priority[MAX_SENSOR_COUNT] {}; + + int8_t _selected_sensor_sub_index{-1}; + + bool _armed{false}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mag_comp_typ, + (ParamBool) _param_sens_mag_mode, + (ParamFloat) _param_sens_mag_rate + ) +}; +}; // namespace sensors diff --git a/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py b/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py new file mode 100644 index 0000000..91319c7 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py @@ -0,0 +1,272 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +File: mag_compensation.py +Author: Tanja Baumann +Email: tanja@auterion.com +Github: https://github.com/baumanta +Description: + Computes linear coefficients for mag compensation from thrust and current +Usage: + python mag_compensation.py /path/to/log/logfile.ulg current --instance 1 + +Remark: + If your logfile does not contain some of the topics, e.g.battery_status/current_a + you will have to comment out the corresponding parts in the script +""" + + +import matplotlib.pylab as plt +from mpl_toolkits.mplot3d import Axes3D +from pyulog import ULog +from pyulog.px4 import PX4ULog +from pylab import * +import numpy as np +import textwrap as tw +import argparse + +#arguments +parser = argparse.ArgumentParser(description='Calculate compensation parameters from ulog') + +parser.add_argument('logfile', type=str, nargs='?', default=[], + help='full path to ulog file') +parser.add_argument('type', type=str, nargs='?', choices=['current', 'thrust'], default=[], + help='Power signal used for compensation, supported is "current" or "thrust".') +parser.add_argument('--instance', type=int, nargs='?', default=0, + help='instance of the current or thrust signal to use (0 or 1)') + +args = parser.parse_args() +log_name = args.logfile +comp_type = args.type +comp_instance = args.instance + +#Load the log data (produced by pyulog) +log = ULog(log_name) +pxlog = PX4ULog(log) + +def get_data(topic_name, variable_name, index): + try: + dataset = log.get_dataset(topic_name, index) + return dataset.data[variable_name] + except: + return [] + +def ms2s_list(time_ms_list): + if len(time_ms_list) > 0: + return 1e-6 * time_ms_list + else: + return time_ms_list + +# Select msgs and copy into arrays +armed = get_data('vehicle_status', 'arming_state', 0) +t_armed = ms2s_list(get_data('vehicle_status', 'timestamp', 0)) + +if comp_type == "thrust": + power = get_data('vehicle_rates_setpoint', 'thrust_body[2]', comp_instance) + power_t = ms2s_list(get_data('vehicle_rates_setpoint', 'timestamp', comp_instance)) + comp_type_param = 1 + factor = 1 + unit = "[G]" +elif comp_type == "current": + power = get_data('battery_status', 'current_a', comp_instance) + power = np.true_divide(power, 1000) #kA + power_t = ms2s_list(get_data('battery_status', 'timestamp', comp_instance)) + comp_type_param = 2 + comp_instance + factor = -1 + unit = "[G/kA]" +else: + print("unknown compensation type {}. Supported is either 'thrust' or 'current'.".format(comp_type)) + sys.exit(1) + +if len(power) == 0: + print("could not retrieve power signal from log, zero data points") + sys.exit(1) + +mag0X_body = get_data('sensor_mag', 'x', 0) +mag0Y_body = get_data('sensor_mag', 'y', 0) +mag0Z_body = get_data('sensor_mag', 'z', 0) +t_mag0 = ms2s_list(get_data('sensor_mag', 'timestamp', 0)) +mag0_ID = get_data('sensor_mag', 'device_id', 0) + +mag1X_body = get_data('sensor_mag', 'x', 1) +mag1Y_body = get_data('sensor_mag', 'y', 1) +mag1Z_body = get_data('sensor_mag', 'z', 1) +t_mag1 = ms2s_list(get_data('sensor_mag', 'timestamp', 1)) +mag1_ID = get_data('sensor_mag', 'device_id', 1) + +mag2X_body = get_data('sensor_mag', 'x', 2) +mag2Y_body = get_data('sensor_mag', 'y', 2) +mag2Z_body = get_data('sensor_mag', 'z', 2) +t_mag2 = ms2s_list(get_data('sensor_mag', 'timestamp', 2)) +mag2_ID = get_data('sensor_mag', 'device_id', 2) + +mag3X_body = get_data('sensor_mag', 'x', 3) +mag3Y_body = get_data('sensor_mag', 'y', 3) +mag3Z_body = get_data('sensor_mag', 'z', 3) +t_mag3 = ms2s_list(get_data('sensor_mag', 'timestamp', 3)) +mag3_ID = get_data('sensor_mag', 'device_id', 3) + +magX_body = [] +magY_body = [] +magZ_body = [] +mag_id = [] +t_mag = [] + +if len(mag0X_body) > 0: + magX_body.append(mag0X_body) + magY_body.append(mag0Y_body) + magZ_body.append(mag0Z_body) + t_mag.append(t_mag0) + mag_id.append(mag0_ID[0]) + +if len(mag1X_body) > 0: + magX_body.append(mag1X_body) + magY_body.append(mag1Y_body) + magZ_body.append(mag1Z_body) + t_mag.append(t_mag1) + mag_id.append(mag1_ID[0]) + +if len(mag2X_body) > 0: + magX_body.append(mag2X_body) + magY_body.append(mag2Y_body) + magZ_body.append(mag2Z_body) + t_mag.append(t_mag2) + mag_id.append(mag2_ID[0]) + +if len(mag3X_body) > 0: + magX_body.append(mag3X_body) + magY_body.append(mag3Y_body) + magZ_body.append(mag3Z_body) + t_mag.append(t_mag3) + mag_id.append(mag3_ID[0]) + +n_mag = len(magX_body) + +#log index does not necessarily match mag calibration instance number +calibration_instance = [] +instance_found = False +for idx in range(n_mag): + instance_found = False + for j in range(4): + if mag_id[idx] == log.initial_parameters["CAL_MAG{}_ID".format(j)]: + calibration_instance.append(j) + instance_found = True + if not instance_found: + print('Mag {} calibration instance not found, run compass calibration first.'.format(mag_id[idx])) + +#get first arming sequence from data +start_time = 0 +stop_time = 0 +for i in range(len(armed)-1): + if armed[i] == 1 and armed[i+1] == 2: + start_time = t_armed[i+1] + if armed[i] == 2 and armed[i+1] == 1: + stop_time = t_armed[i+1] + break + +#cut unarmed sequences from mag data +index_start = 0 +index_stop = 0 + +for idx in range(n_mag): + for i in range(len(t_mag[idx])): + if t_mag[idx][i] > start_time: + index_start = i + break + + for i in range(len(t_mag[idx])): + if t_mag[idx][i] > stop_time: + index_stop = i -1 + break + + t_mag[idx] = t_mag[idx][index_start:index_stop] + magX_body[idx] = magX_body[idx][index_start:index_stop] + magY_body[idx] = magY_body[idx][index_start:index_stop] + magZ_body[idx] = magZ_body[idx][index_start:index_stop] + + +#resample data +power_resampled = [] + +for idx in range(n_mag): + power_resampled.append(interp(t_mag[idx], power_t, power)) + +#fit linear to get coefficients +px = [] +py = [] +pz = [] + +for idx in range(n_mag): + px_temp, res_x, _, _, _ = polyfit(power_resampled[idx], magX_body[idx], 1,full = True) + py_temp, res_y, _, _, _ = polyfit(power_resampled[idx], magY_body[idx], 1,full = True) + pz_temp, res_z, _, _, _ = polyfit(power_resampled[idx], magZ_body[idx], 1, full = True) + + px.append(px_temp) + py.append(py_temp) + pz.append(pz_temp) + +#print to console +for idx in range(n_mag): + print('Mag{} device ID {} (calibration instance {})'.format(idx, mag_id[idx], calibration_instance[idx])) +print('\033[91m \n{}-based compensation: \033[0m'.format(comp_type)) +print('\nparam set CAL_MAG_COMP_TYP {}'.format(comp_type_param)) +for idx in range(n_mag): + print('\nparam set CAL_MAG{}_XCOMP {:.3f}'.format(calibration_instance[idx], factor * px[idx][0])) + print('param set CAL_MAG{}_YCOMP {:.3f}'.format(calibration_instance[idx], factor * py[idx][0])) + print('param set CAL_MAG{}_ZCOMP {:.3f}'.format(calibration_instance[idx], factor * pz[idx][0])) + +#plot data + +for idx in range(n_mag): + fig = plt.figure(num=None, figsize=(25, 14), dpi=80, facecolor='w', edgecolor='k') + fig.suptitle('Compensation Parameter Fit \n{} \nmag {} ID: {} (calibration instance {})'.format(log_name, idx, mag_id[idx], calibration_instance[idx]), fontsize=14, fontweight='bold') + + plt.subplot(1,3,1) + plt.plot(power_resampled[idx], magX_body[idx], 'yo', power_resampled[idx], px[idx][0]*power_resampled[idx]+px[idx][1], '--k') + plt.xlabel('current [kA]') + plt.ylabel('mag X [G]') + + plt.subplot(1,3,2) + plt.plot(power_resampled[idx], magY_body[idx], 'yo', power_resampled[idx], py[idx][0]*power_resampled[idx]+py[idx][1], '--k') + plt.xlabel('current [kA]') + plt.ylabel('mag Y [G]') + + plt.subplot(1,3,3) + plt.plot(power_resampled[idx], magZ_body[idx], 'yo', power_resampled[idx], pz[idx][0]*power_resampled[idx]+pz[idx][1], '--k') + plt.xlabel('current [kA]') + plt.ylabel('mag Z [G]') + + # display results + plt.figtext(0.24, 0.03, 'CAL_MAG{}_XCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * px[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold') + plt.figtext(0.51, 0.03, 'CAL_MAG{}_YCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * py[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold') + plt.figtext(0.79, 0.03, 'CAL_MAG{}_ZCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * pz[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold') + + +#compensation comparison plots +for idx in range(n_mag): + fig = plt.figure(num=None, figsize=(25, 14), dpi=80, facecolor='w', edgecolor='k') + fig.suptitle('Original Data vs. Compensation \n{}\nmag {} ID: {} (calibration instance {})'.format(log_name, idx, mag_id[idx], calibration_instance[idx]), fontsize=14, fontweight='bold') + + plt.subplot(3,1,1) + original_x, = plt.plot(t_mag[idx], magX_body[idx], label='original') + power_x, = plt.plot(t_mag[idx],magX_body[idx] - px[idx][0] * power_resampled[idx], label='compensated') + plt.legend(handles=[original_x, power_x]) + plt.xlabel('Time [s]') + plt.ylabel('Mag X corrected[G]') + + plt.subplot(3,1,2) + original_y, = plt.plot(t_mag[idx], magY_body[idx], label='original') + power_y, = plt.plot(t_mag[idx],magY_body[idx] - py[idx][0] * power_resampled[idx], label='compensated') + plt.legend(handles=[original_y, power_y]) + plt.xlabel('Time [s]') + plt.ylabel('Mag Y corrected[G]') + + plt.subplot(3,1,3) + original_z, = plt.plot(t_mag[idx], magZ_body[idx], label='original') + power_z, = plt.plot(t_mag[idx],magZ_body[idx] - pz[idx][0] * power_resampled[idx], label='compensated') + plt.legend(handles=[original_z, power_z]) + plt.xlabel('Time [s]') + plt.ylabel('Mag Z corrected[G]') + +plt.show() diff --git a/src/prometheus_px4/src/modules/sensors/voted_sensors_update.cpp b/src/prometheus_px4/src/modules/sensors/voted_sensors_update.cpp new file mode 100644 index 0000000..fff9142 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/voted_sensors_update.cpp @@ -0,0 +1,458 @@ +/**************************************************************************** + * + * Copyright (c) 2016, 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 voted_sensors_update.cpp + * + * @author Beat Kueng + */ + +#include "voted_sensors_update.h" + +#include +#include +#include +#include + +using namespace sensors; +using namespace matrix; +using namespace time_literals; + +VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, + uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) : + ModuleParams(nullptr), + _vehicle_imu_sub(vehicle_imu_sub), + _hil_enabled(hil_enabled) +{ + if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit + _gyro.voter.set_timeout(500000); + _accel.voter.set_timeout(500000); + } +} + +int VotedSensorsUpdate::init(sensor_combined_s &raw) +{ + raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + raw.timestamp = 0; + + initializeSensors(); + + _selection_changed = true; + + return 0; +} + +void VotedSensorsUpdate::initializeSensors() +{ + initSensorClass(_gyro, MAX_SENSOR_COUNT); + initSensorClass(_accel, MAX_SENSOR_COUNT); +} + +void VotedSensorsUpdate::parametersUpdate() +{ + updateParams(); + + // run through all IMUs + for (uint8_t uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { + uORB::SubscriptionData imu{ORB_ID(vehicle_imu), uorb_index}; + imu.update(); + + if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) { + + // find corresponding configured accel priority + int8_t accel_cal_index = calibration::FindCalibrationIndex("ACC", imu.get().accel_device_id); + + if (accel_cal_index >= 0) { + // found matching CAL_ACCx_PRIO + int32_t accel_priority_old = _accel.priority_configured[uorb_index]; + + _accel.priority_configured[uorb_index] = calibration::GetCalibrationParam("ACC", "PRIO", accel_cal_index); + + if (accel_priority_old != _accel.priority_configured[uorb_index]) { + if (_accel.priority_configured[uorb_index] == 0) { + // disabled + _accel.priority[uorb_index] = 0; + + } else { + // change relative priority to incorporate any sensor faults + int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old; + _accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, static_cast(1), + static_cast(100)); + } + } + } + + // find corresponding configured gyro priority + int8_t gyro_cal_index = calibration::FindCalibrationIndex("GYRO", imu.get().gyro_device_id); + + if (gyro_cal_index >= 0) { + // found matching CAL_GYROx_PRIO + int32_t gyro_priority_old = _gyro.priority_configured[uorb_index]; + + _gyro.priority_configured[uorb_index] = calibration::GetCalibrationParam("GYRO", "PRIO", gyro_cal_index); + + if (gyro_priority_old != _gyro.priority_configured[uorb_index]) { + if (_gyro.priority_configured[uorb_index] == 0) { + // disabled + _gyro.priority[uorb_index] = 0; + + } else { + // change relative priority to incorporate any sensor faults + int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old; + _gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, static_cast(1), + static_cast(100)); + } + } + } + } + } +} + +void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) +{ + for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { + vehicle_imu_s imu_report; + + if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0) + && _vehicle_imu_sub[uorb_index].update(&imu_report)) { + + // copy corresponding vehicle_imu_status for accel & gyro error counts + vehicle_imu_status_s imu_status{}; + _vehicle_imu_status_subs[uorb_index].copy(&imu_status); + + _accel_device_id[uorb_index] = imu_report.accel_device_id; + _gyro_device_id[uorb_index] = imu_report.gyro_device_id; + + // convert the delta velocities to an equivalent acceleration + const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt; + Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv; + + + // convert the delta angles to an equivalent angular rate + const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt; + Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv; + + _last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample; + _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); + _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); + _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); + _last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt; + _last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping; + _last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0); + _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); + _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); + _last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt; + + + _last_accel_timestamp[uorb_index] = imu_report.timestamp_sample; + + _accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, + imu_status.accel_error_count, _accel.priority[uorb_index]); + + _gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, + imu_status.gyro_error_count, _gyro.priority[uorb_index]); + } + } + + // find the best sensor + int accel_best_index = -1; + int gyro_best_index = -1; + _accel.voter.get_best(hrt_absolute_time(), &accel_best_index); + _gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index); + + if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) { + // use sensor_selection to find best + if (_sensor_selection_sub.update(&_selection)) { + // reset inconsistency checks against primary + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + _accel_diff[sensor_index].zero(); + } + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + _gyro_diff[sensor_index].zero(); + } + } + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) { + accel_best_index = i; + } + + if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) { + gyro_best_index = i; + } + } + + } else { + // use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published + checkFailover(_accel, "Accel"); + checkFailover(_gyro, "Gyro"); + } + + // write data for the best sensor to output variables + if ((accel_best_index >= 0) && (gyro_best_index >= 0)) { + raw.timestamp = _last_sensor_data[gyro_best_index].timestamp; + memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2, + sizeof(raw.accelerometer_m_s2)); + memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad)); + raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt; + raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; + raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; + + if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) { + _accel.last_best_vote = (uint8_t)accel_best_index; + _selection.accel_device_id = _accel_device_id[accel_best_index]; + _selection_changed = true; + } + + if ((_gyro.last_best_vote != gyro_best_index) || (_selection.gyro_device_id != _gyro_device_id[gyro_best_index])) { + _gyro.last_best_vote = (uint8_t)gyro_best_index; + _selection.gyro_device_id = _gyro_device_id[gyro_best_index]; + _selection_changed = true; + + // clear all registered callbacks + for (auto &sub : _vehicle_imu_sub) { + sub.unregisterCallback(); + } + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + vehicle_imu_s report{}; + + if (_vehicle_imu_sub[i].copy(&report)) { + if ((report.gyro_device_id != 0) && (report.gyro_device_id == _gyro_device_id[gyro_best_index])) { + _vehicle_imu_sub[i].registerCallback(); + } + } + } + } + } + + // publish sensor selection if changed + if (_param_sens_imu_mode.get() || (_selection.timestamp == 0)) { + if (_selection_changed) { + // don't publish until selected IDs are valid + if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) { + _selection.timestamp = hrt_absolute_time(); + _sensor_selection_pub.publish(_selection); + _selection_changed = false; + } + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + _accel_diff[sensor_index].zero(); + _gyro_diff[sensor_index].zero(); + } + } + } +} + +bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name) +{ + if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) { + + uint32_t flags = sensor.voter.failover_state(); + int failover_index = sensor.voter.failover_index(); + + if (flags == DataValidator::ERROR_FLAG_NO_ERROR) { + if (failover_index != -1) { + // we switched due to a non-critical reason. No need to panic. + PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index); + } + + } else { + if (failover_index != -1) { + + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_error_message > 3_s) { + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!", + sensor_name, + failover_index, + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + _last_error_message = now; + } + + // reduce priority of failed sensor to the minimum + sensor.priority[failover_index] = 1; + } + } + + sensor.last_failover_count = sensor.voter.failover_count(); + return true; + } + + return false; +} + +void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max) +{ + bool added = false; + int max_sensor_index = -1; + + for (unsigned i = 0; i < sensor_count_max; i++) { + + max_sensor_index = i; + + if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) { + sensor_data.advertised[i] = true; + sensor_data.priority[i] = DEFAULT_PRIORITY; + sensor_data.priority_configured[i] = DEFAULT_PRIORITY; + + if (i > 0) { + /* the first always exists, but for each further sensor, add a new validator */ + if (sensor_data.voter.add_new_validator()) { + added = true; + + } else { + PX4_ERR("failed to add validator for sensor %s %i", sensor_data.subscription[i].get_topic()->o_name, i); + } + } + } + } + + // never decrease the sensor count, as we could end up with mismatching validators + if (max_sensor_index + 1 > sensor_data.subscription_count) { + sensor_data.subscription_count = max_sensor_index + 1; + } + + if (added) { + // force parameter refresh if anything was added + parametersUpdate(); + } +} + +void VotedSensorsUpdate::printStatus() +{ + PX4_INFO("selected gyro: %" PRIu32 " (%" PRIu8 ")", _selection.gyro_device_id, _gyro.last_best_vote); + _gyro.voter.print(); + + PX4_INFO_RAW("\n"); + PX4_INFO("selected accel: %" PRIu32 " (%" PRIu8 ")", _selection.accel_device_id, _accel.last_best_vote); + _accel.voter.print(); +} + +void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) +{ + imuPoll(raw); + + calcAccelInconsistency(); + calcGyroInconsistency(); + + sensors_status_imu_s status{}; + status.accel_device_id_primary = _selection.accel_device_id; + status.gyro_device_id_primary = _selection.gyro_device_id; + + static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof( + sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); + static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( + sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if ((_accel_device_id[i] != 0) && (_accel.priority[i] > 0)) { + status.accel_device_ids[i] = _accel_device_id[i]; + status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm(); + status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); + } + + if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) { + status.gyro_device_ids[i] = _gyro_device_id[i]; + status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm(); + status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); + } + } + + + status.timestamp = hrt_absolute_time(); + _sensors_status_imu_pub.publish(status); +} + +void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) +{ + if (_last_accel_timestamp[_accel.last_best_vote]) { + raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] - + (int64_t)raw.timestamp); + } +} + +void VotedSensorsUpdate::calcAccelInconsistency() +{ + Vector3f accel_mean{}; + Vector3f accel_all[MAX_SENSOR_COUNT] {}; + uint8_t accel_count = 0; + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { + accel_count++; + accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2}; + accel_mean += accel_all[sensor_index]; + } + } + + if (accel_count > 0) { + accel_mean /= accel_count; + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { + _accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean); + } + } + } +} + +void VotedSensorsUpdate::calcGyroInconsistency() +{ + Vector3f gyro_mean{}; + Vector3f gyro_all[MAX_SENSOR_COUNT] {}; + uint8_t gyro_count = 0; + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { + gyro_count++; + gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad}; + gyro_mean += gyro_all[sensor_index]; + } + } + + if (gyro_count > 0) { + gyro_mean /= gyro_count; + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { + _gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean); + } + } + } +} diff --git a/src/prometheus_px4/src/modules/sensors/voted_sensors_update.h b/src/prometheus_px4/src/modules/sensors/voted_sensors_update.h new file mode 100644 index 0000000..750a3d5 --- /dev/null +++ b/src/prometheus_px4/src/modules/sensors/voted_sensors_update.h @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 + +/** + * @file voted_sensors_update.h + * + * @author Beat Kueng + */ + +#include "data_validator/DataValidator.hpp" +#include "data_validator/DataValidatorGroup.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sensors +{ + +static constexpr uint8_t MAX_SENSOR_COUNT = 4; + +/** + ** class VotedSensorsUpdate + * + * Handling of sensor updates with voting + */ +class VotedSensorsUpdate : public ModuleParams +{ +public: + /** + * @param parameters parameter values. These do not have to be initialized when constructing this object. + * Only when calling init(), they have to be initialized. + */ + VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]); + + /** + * initialize subscriptions etc. + * @return 0 on success, <0 otherwise + */ + int init(sensor_combined_s &raw); + + /** + * This tries to find new sensor instances. This is called from init(), then it can be called periodically. + */ + void initializeSensors(); + + void printStatus(); + + /** + * call this whenever parameters got updated. Make sure to have initializeSensors() called at least + * once before calling this. + */ + void parametersUpdate(); + + /** + * read new sensor data + */ + void sensorsPoll(sensor_combined_s &raw); + + /** + * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, + * so that the data can be published. + */ + void setRelativeTimestamps(sensor_combined_s &raw); + +private: + + static constexpr uint8_t DEFAULT_PRIORITY = 50; + + struct SensorData { + SensorData() = delete; + explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {} + + uORB::Subscription subscription[MAX_SENSOR_COUNT]; /**< raw sensor data subscription */ + DataValidatorGroup voter{1}; + unsigned int last_failover_count{0}; + int32_t priority[MAX_SENSOR_COUNT] {}; + int32_t priority_configured[MAX_SENSOR_COUNT] {}; + uint8_t last_best_vote{0}; /**< index of the latest best vote */ + uint8_t subscription_count{0}; + bool advertised[MAX_SENSOR_COUNT] {false, false, false}; + }; + + void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max); + + /** + * Poll IMU for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void imuPoll(sensor_combined_s &raw); + + /** + * Check & handle failover of a sensor + * @return true if a switch occured (could be for a non-critical reason) + */ + bool checkFailover(SensorData &sensor, const char *sensor_name); + + /** + * Calculates the magnitude in m/s/s of the largest difference between each accelerometer vector and the mean of all vectors + */ + void calcAccelInconsistency(); + + /** + * Calculates the magnitude in rad/s of the largest difference between each gyro vector and the mean of all vectors + */ + void calcGyroInconsistency(); + + SensorData _accel{ORB_ID::sensor_accel}; + SensorData _gyro{ORB_ID::sensor_gyro}; + + hrt_abstime _last_error_message{0}; + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ + uORB::Publication _sensors_status_imu_pub{ORB_ID(sensors_status_imu)}; + + uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[MAX_SENSOR_COUNT]; + uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; + + uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; + + sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {}; /**< latest sensor data from all sensors instances */ + + const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ + + bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */ + + matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {}; /**< filtered accel differences between IMU units (m/s/s) */ + matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {}; /**< filtered gyro differences between IMU uinits (rad/s) */ + + uint32_t _accel_device_id[MAX_SENSOR_COUNT] {}; /**< accel driver device id for each uorb instance */ + uint32_t _gyro_device_id[MAX_SENSOR_COUNT] {}; /**< gyro driver device id for each uorb instance */ + + uint64_t _last_accel_timestamp[MAX_SENSOR_COUNT] {}; /**< latest full timestamp */ + + sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ + + DEFINE_PARAMETERS( + (ParamBool) _param_sens_imu_mode + ) +}; + +} /* namespace sensors */ diff --git a/src/prometheus_px4/src/modules/sih/CMakeLists.txt b/src/prometheus_px4/src/modules/sih/CMakeLists.txt new file mode 100644 index 0000000..fce939c --- /dev/null +++ b/src/prometheus_px4/src/modules/sih/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__sih + MAIN sih + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + sih.cpp + sih.hpp + DEPENDS + mathlib + drivers_accelerometer + drivers_barometer + drivers_gyroscope + drivers_magnetometer + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/sih/sih.cpp b/src/prometheus_px4/src/modules/sih/sih.cpp new file mode 100644 index 0000000..3cdac73 --- /dev/null +++ b/src/prometheus_px4/src/modules/sih/sih.cpp @@ -0,0 +1,533 @@ +/**************************************************************************** + * + * 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 sih.cpp + * Simulator in Hardware + * + * @author Romain Chiappinelli + * + * Coriolis g Corporation - January 2019 + */ + +#include "sih.hpp" + +#include +#include + +#include // to get PWM flags +#include + +using namespace math; +using namespace matrix; +using namespace time_literals; + +Sih::Sih() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + _px4_accel.set_temperature(T1_C); + _px4_gyro.set_temperature(T1_C); + _px4_mag.set_temperature(T1_C); + + parameters_updated(); + init_variables(); + gps_no_fix(); + + const hrt_abstime task_start = hrt_absolute_time(); + _last_run = task_start; + _gps_time = task_start; + _gt_time = task_start; + _dist_snsr_time = task_start; +} + +Sih::~Sih() +{ + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +bool Sih::init() +{ + int rate = _imu_gyro_ratemax.get(); + + // default to 250 Hz (4000 us interval) + if (rate <= 0) { + rate = 250; + } + + // 200 - 2000 Hz + int interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000); + ScheduleOnInterval(interval_us); + + return true; +} + +void Sih::Run() +{ + perf_count(_loop_interval_perf); + + // 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(); + parameters_updated(); + } + + perf_begin(_loop_perf); + + _now = hrt_absolute_time(); + _dt = (_now - _last_run) * 1e-6f; + _last_run = _now; + + read_motors(); + + generate_force_and_torques(); + + equations_of_motion(); + + reconstruct_sensors_signals(); + + // update IMU every iteration + _px4_accel.update(_now, _acc(0), _acc(1), _acc(2)); + _px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2)); + + // magnetometer published at 50 Hz + if (_now - _mag_time >= 20_ms + && fabs(_mag_offset_x) < 10000 + && fabs(_mag_offset_y) < 10000 + && fabs(_mag_offset_z) < 10000) { + _mag_time = _now; + _px4_mag.update(_now, _mag(0), _mag(1), _mag(2)); + } + + // baro published at 20 Hz + if (_now - _baro_time >= 50_ms + && fabs(_baro_offset_m) < 10000) { + _baro_time = _now; + _px4_baro.set_temperature(_baro_temp_c); + _px4_baro.update(_now, _baro_p_mBar); + } + + // gps published at 20Hz + if (_now - _gps_time >= 50_ms) { + _gps_time = _now; + send_gps(); + } + + // distance sensor published at 50 Hz + if (_now - _dist_snsr_time >= 20_ms + && fabs(_distance_snsr_override) < 10000) { + _dist_snsr_time = _now; + send_dist_snsr(); + } + + // send groundtruth message every 40 ms + if (_now - _gt_time >= 40_ms) { + _gt_time = _now; + + publish_sih(); // publish _sih message for debug purpose + } + + perf_end(_loop_perf); +} + +// store the parameters in a more convenient form +void Sih::parameters_updated() +{ + _T_MAX = _sih_t_max.get(); + _Q_MAX = _sih_q_max.get(); + _L_ROLL = _sih_l_roll.get(); + _L_PITCH = _sih_l_pitch.get(); + _KDV = _sih_kdv.get(); + _KDW = _sih_kdw.get(); + _H0 = _sih_h0.get(); + + _LAT0 = (double)_sih_lat0.get() * 1.0e-7; + _LON0 = (double)_sih_lon0.get() * 1.0e-7; + _COS_LAT0 = cosl((long double)radians(_LAT0)); + + _MASS = _sih_mass.get(); + + _W_I = Vector3f(0.0f, 0.0f, _MASS * CONSTANTS_ONE_G); + + _I = diag(Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get())); + _I(0, 1) = _I(1, 0) = _sih_ixy.get(); + _I(0, 2) = _I(2, 0) = _sih_ixz.get(); + _I(1, 2) = _I(2, 1) = _sih_iyz.get(); + + _Im1 = inv(_I); + + _mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get()); + + _gps_used = _sih_gps_used.get(); + _baro_offset_m = _sih_baro_offset.get(); + _mag_offset_x = _sih_mag_offset_x.get(); + _mag_offset_y = _sih_mag_offset_y.get(); + _mag_offset_z = _sih_mag_offset_z.get(); + + _distance_snsr_min = _sih_distance_snsr_min.get(); + _distance_snsr_max = _sih_distance_snsr_max.get(); + _distance_snsr_override = _sih_distance_snsr_override.get(); + + _T_TAU = _sih_thrust_tau.get(); +} + +// initialization of the variables for the simulator +void Sih::init_variables() +{ + srand(1234); // initialize the random seed once before calling generate_wgn() + + _p_I = Vector3f(0.0f, 0.0f, 0.0f); + _v_I = Vector3f(0.0f, 0.0f, 0.0f); + _q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + _w_B = Vector3f(0.0f, 0.0f, 0.0f); + + _u[0] = _u[1] = _u[2] = _u[3] = 0.0f; +} + +void Sih::gps_fix() +{ + _sensor_gps.fix_type = 3; // 3D fix + _sensor_gps.satellites_used = _gps_used; + _sensor_gps.heading = NAN; + _sensor_gps.heading_offset = NAN; + _sensor_gps.s_variance_m_s = 0.5f; + _sensor_gps.c_variance_rad = 0.1f; + _sensor_gps.eph = 0.9f; + _sensor_gps.epv = 1.78f; + _sensor_gps.hdop = 0.7f; + _sensor_gps.vdop = 1.1f; +} + +void Sih::gps_no_fix() +{ + _sensor_gps.fix_type = 0; // 3D fix + _sensor_gps.satellites_used = _gps_used; + _sensor_gps.heading = NAN; + _sensor_gps.heading_offset = NAN; + _sensor_gps.s_variance_m_s = 100.f; + _sensor_gps.c_variance_rad = 100.f; + _sensor_gps.eph = 100.f; + _sensor_gps.epv = 100.f; + _sensor_gps.hdop = 100.f; + _sensor_gps.vdop = 100.f; +} + + +// read the motor signals outputted from the mixer +void Sih::read_motors() +{ + actuator_outputs_s actuators_out; + + if (_actuator_out_sub.update(&actuators_out)) { + for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals + float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f); + _u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau + } + } +} + +// generate the motors thrust and torque in the body frame +void Sih::generate_force_and_torques() +{ + _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); + _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), + _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), + _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); + + _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft + _Ma_B = -_KDW * _w_B; // first order angular damper +} + +// apply the equations of motion of a rigid body and integrate one step +void Sih::equations_of_motion() +{ + _C_IB = matrix::Dcm(_q); // body to inertial transformation + + // Equations of motion of a rigid body + _p_I_dot = _v_I; // position differential + _v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum + _q_dot = _q.derivative1(_w_B); // attitude differential + _w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum + + // fake ground, avoid free fall + if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) { + if (!_grounded) { // if we just hit the floor + // for the accelerometer, compute the acceleration that will stop the vehicle in one time step + _v_I_dot = -_v_I / _dt; + + } else { + _v_I_dot.setZero(); + } + + _v_I.setZero(); + _w_B.setZero(); + _grounded = true; + + } else { + // integration: Euler forward + _p_I = _p_I + _p_I_dot * _dt; + _v_I = _v_I + _v_I_dot * _dt; + _q = _q + _q_dot * _dt; // as given in attitude_estimator_q_main.cpp + _q.normalize(); + _w_B = _w_B + _w_B_dot * _dt; + _grounded = false; + } +} + +// reconstruct the noisy sensor signals +void Sih::reconstruct_sensors_signals() +{ + // The sensor signals reconstruction and noise levels are from + // Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." + // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. + + // IMU + _acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f); + _gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f); + _mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f); + _mag(0) += _mag_offset_x; + _mag(1) += _mag_offset_y; + _mag(2) += _mag_offset_z; + + // barometer + float altitude = (_H0 - _p_I(2)) + _baro_offset_m + generate_wgn() * 0.14f; // altitude with noise + _baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR * // reconstructed pressure in mBar + powf((1.0f + altitude * TEMP_GRADIENT / T1_K), -CONSTANTS_ONE_G / (TEMP_GRADIENT * CONSTANTS_AIR_GAS_CONST)); + _baro_temp_c = T1_K + CONSTANTS_ABSOLUTE_NULL_CELSIUS + TEMP_GRADIENT * altitude; // reconstructed temperture in celcius + + // GPS + _gps_lat_noiseless = _LAT0 + degrees((double)_p_I(0) / CONSTANTS_RADIUS_OF_EARTH); + _gps_lon_noiseless = _LON0 + degrees((double)_p_I(1) / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0; + _gps_alt_noiseless = _H0 - _p_I(2); + + _gps_lat = _gps_lat_noiseless + degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); + _gps_lon = _gps_lon_noiseless + degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0; + _gps_alt = _gps_alt_noiseless + generate_wgn() * 0.5f; + _gps_vel = _v_I + noiseGauss3f(0.06f, 0.077f, 0.158f); +} + +void Sih::send_gps() +{ + _sensor_gps.timestamp = _now; + _sensor_gps.lat = (int32_t)(_gps_lat * 1e7); // Latitude in 1E-7 degrees + _sensor_gps.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees + _sensor_gps.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres) + _sensor_gps.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres) + _sensor_gps.vel_ned_valid = true; // True if NED velocity is valid + _sensor_gps.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel( + 1)); // GPS ground speed, (metres/sec) + _sensor_gps.vel_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec) + _sensor_gps.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec) + _sensor_gps.vel_d_m_s = _gps_vel(2); // GPS Down velocity, (metres/sec) + _sensor_gps.cog_rad = atan2(_gps_vel(1), + _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) + + if (_gps_used >= 4) { + gps_fix(); + + } else { + gps_no_fix(); + } + + // device id + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = 0; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + _sensor_gps.device_id = device_id.devid; + + _sensor_gps_pub.publish(_sensor_gps); +} + +void Sih::send_dist_snsr() +{ + _distance_snsr.timestamp = _now; + _distance_snsr.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + _distance_snsr.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + _distance_snsr.min_distance = _distance_snsr_min; + _distance_snsr.max_distance = _distance_snsr_max; + _distance_snsr.signal_quality = -1; + _distance_snsr.device_id = 0; + + if (_distance_snsr_override >= 0.f) { + _distance_snsr.current_distance = _distance_snsr_override; + + } else { + _distance_snsr.current_distance = -_p_I(2) / _C_IB(2, 2); + + if (_distance_snsr.current_distance > _distance_snsr_max) { + // this is based on lightware lw20 behaviour + _distance_snsr.current_distance = UINT16_MAX / 100.f; + + } + } + + _distance_snsr_pub.publish(_distance_snsr); +} + +void Sih::publish_sih() +{ + // publish angular velocity groundtruth + _vehicle_angular_velocity_gt.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_gt.xyz[0] = _w_B(0); // rollspeed; + _vehicle_angular_velocity_gt.xyz[1] = _w_B(1); // pitchspeed; + _vehicle_angular_velocity_gt.xyz[2] = _w_B(2); // yawspeed; + + _vehicle_angular_velocity_gt_pub.publish(_vehicle_angular_velocity_gt); + + // publish attitude groundtruth + _att_gt.timestamp = hrt_absolute_time(); + _att_gt.q[0] = _q(0); + _att_gt.q[1] = _q(1); + _att_gt.q[2] = _q(2); + _att_gt.q[3] = _q(3); + + _att_gt_pub.publish(_att_gt); + + // publish position groundtruth + _gpos_gt.timestamp = hrt_absolute_time(); + _gpos_gt.lat = _gps_lat_noiseless; + _gpos_gt.lon = _gps_lon_noiseless; + _gpos_gt.alt = _gps_alt_noiseless; + + _gpos_gt_pub.publish(_gpos_gt); +} + +float Sih::generate_wgn() // generate white Gaussian noise sample with std=1 +{ + // algorithm 1: + // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); + // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); + // algorithm 2: from BlockRandGauss.hpp + static float V1, V2, S; + static bool phase = true; + float X; + + if (phase) { + do { + float U1 = (float)rand() / RAND_MAX; + float U2 = (float)rand() / RAND_MAX; + V1 = 2.0f * U1 - 1.0f; + V2 = 2.0f * U2 - 1.0f; + S = V1 * V1 + V2 * V2; + } while (S >= 1.0f || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); + } + + phase = !phase; + return X; +} + +// generate white Gaussian noise sample vector with specified std +Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz) +{ + return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); +} + +int Sih::task_spawn(int argc, char *argv[]) +{ + Sih *instance = new Sih(); + + 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 Sih::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int Sih::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module provide a simulator for quadrotors running fully +inside the hardware autopilot. + +This simulator subscribes to "actuator_outputs" which are the actuator pwm +signals given by the mixer. + +This simulator publishes the sensors signals corrupted with realistic noise +in order to incorporate the state estimator in the loop. + +### Implementation +The simulator implements the equations of motion using matrix algebra. +Quaternion representation is used for the attitude. +Forward Euler is used for integration. +Most of the variables are declared global in the .hpp file to avoid stack overflow. + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sih", "simulation"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sih_main(int argc, char *argv[]) +{ + return Sih::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/sih/sih.hpp b/src/prometheus_px4/src/modules/sih/sih.hpp new file mode 100644 index 0000000..2b6e477 --- /dev/null +++ b/src/prometheus_px4/src/modules/sih/sih.hpp @@ -0,0 +1,222 @@ +/**************************************************************************** +* +* 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. +* +****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include // matrix, vectors, dcm, quaterions +#include // math::radians, +#include // to get the physical constants +#include // to get the real time +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // to publish groundtruth +#include // to publish groundtruth +#include // to publish groundtruth +#include + +using namespace time_literals; + +class Sih : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + Sih(); + ~Sih() 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); + + static float generate_wgn(); // generate white Gaussian noise sample + + // generate white Gaussian noise sample as a 3D vector with specified std + static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz); + + bool init(); + +private: + void Run() override; + + void parameters_updated(); + + // simulated sensor instances + PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + + // to publish the gps position + sensor_gps_s _sensor_gps{}; + uORB::Publication _sensor_gps_pub{ORB_ID(sensor_gps)}; + + // to publish the distance sensor + distance_sensor_s _distance_snsr{}; + uORB::Publication _distance_snsr_pub{ORB_ID(distance_sensor)}; + + // angular velocity groundtruth + vehicle_angular_velocity_s _vehicle_angular_velocity_gt{}; + uORB::Publication _vehicle_angular_velocity_gt_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; + + // attitude groundtruth + vehicle_attitude_s _att_gt{}; + uORB::Publication _att_gt_pub{ORB_ID(vehicle_attitude_groundtruth)}; + + // global position groundtruth + vehicle_global_position_s _gpos_gt{}; + uORB::Publication _gpos_gt_pub{ORB_ID(vehicle_global_position_groundtruth)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; + + // hard constants + static constexpr uint16_t NB_MOTORS = 4; + static constexpr float T1_C = 15.0f; // ground temperature in celcius + static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin + static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre + + void init_variables(); + void gps_fix(); + void gps_no_fix(); + void read_motors(); + void generate_force_and_torques(); + void equations_of_motion(); + void reconstruct_sensors_signals(); + void send_gps(); + void send_dist_snsr(); + void publish_sih(); + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + + hrt_abstime _last_run{0}; + hrt_abstime _baro_time{0}; + hrt_abstime _gps_time{0}; + hrt_abstime _mag_time{0}; + hrt_abstime _gt_time{0}; + hrt_abstime _dist_snsr_time{0}; + hrt_abstime _now{0}; + float _dt{0}; // sampling time [s] + bool _grounded{true};// whether the vehicle is on the ground + + matrix::Vector3f _T_B; // thrust force in body frame [N] + matrix::Vector3f _Fa_I; // aerodynamic force in inertial frame [N] + matrix::Vector3f _Mt_B; // thruster moments in the body frame [Nm] + matrix::Vector3f _Ma_B; // aerodynamic moments in the body frame [Nm] + matrix::Vector3f _p_I; // inertial position [m] + matrix::Vector3f _v_I; // inertial velocity [m/s] + matrix::Vector3f _v_B; // body frame velocity [m/s] + matrix::Vector3f _p_I_dot; // inertial position differential + matrix::Vector3f _v_I_dot; // inertial velocity differential + matrix::Quatf _q; // quaternion attitude + matrix::Dcmf _C_IB; // body to inertial transformation + matrix::Vector3f _w_B; // body rates in body frame [rad/s] + matrix::Quatf _q_dot; // quaternion differential + matrix::Vector3f _w_B_dot; // body rates differential + float _u[NB_MOTORS]; // thruster signals + + + // sensors reconstruction + matrix::Vector3f _acc; + matrix::Vector3f _mag; + matrix::Vector3f _gyro; + matrix::Vector3f _gps_vel; + double _gps_lat, _gps_lat_noiseless; + double _gps_lon, _gps_lon_noiseless; + float _gps_alt, _gps_alt_noiseless; + float _baro_p_mBar; // reconstructed (simulated) pressure in mBar + float _baro_temp_c; // reconstructed (simulated) barometer temperature in celcius + + // parameters + float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0, _T_TAU; + double _LAT0, _LON0, _COS_LAT0; + matrix::Vector3f _W_I; // weight of the vehicle in inertial frame [N] + matrix::Matrix3f _I; // vehicle inertia matrix + matrix::Matrix3f _Im1; // inverse of the intertia matrix + matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G] + + int _gps_used; + float _baro_offset_m, _mag_offset_x, _mag_offset_y, _mag_offset_z; + float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override; + + // parameters defined in sih_params.c + DEFINE_PARAMETERS( + (ParamInt) _imu_gyro_ratemax, + + (ParamFloat) _sih_mass, + (ParamFloat) _sih_ixx, + (ParamFloat) _sih_iyy, + (ParamFloat) _sih_izz, + (ParamFloat) _sih_ixy, + (ParamFloat) _sih_ixz, + (ParamFloat) _sih_iyz, + (ParamFloat) _sih_t_max, + (ParamFloat) _sih_q_max, + (ParamFloat) _sih_l_roll, + (ParamFloat) _sih_l_pitch, + (ParamFloat) _sih_kdv, + (ParamFloat) _sih_kdw, + (ParamInt) _sih_lat0, + (ParamInt) _sih_lon0, + (ParamFloat) _sih_h0, + (ParamFloat) _sih_mu_x, + (ParamFloat) _sih_mu_y, + (ParamFloat) _sih_mu_z, + (ParamInt) _sih_gps_used, + (ParamFloat) _sih_baro_offset, + (ParamFloat) _sih_mag_offset_x, + (ParamFloat) _sih_mag_offset_y, + (ParamFloat) _sih_mag_offset_z, + (ParamFloat) _sih_distance_snsr_min, + (ParamFloat) _sih_distance_snsr_max, + (ParamFloat) _sih_distance_snsr_override, + (ParamFloat) _sih_thrust_tau + ) +}; diff --git a/src/prometheus_px4/src/modules/sih/sih_params.c b/src/prometheus_px4/src/modules/sih/sih_params.c new file mode 100644 index 0000000..9254271 --- /dev/null +++ b/src/prometheus_px4/src/modules/sih/sih_params.c @@ -0,0 +1,437 @@ +/**************************************************************************** + * + * 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 sih_params.c + * Parameters for quadcopter X simulator in hardware. + * + * @author Romain Chiappinelli + * February 2019 + */ + +/** + * Vehicle mass + * + * This value can be measured by weighting the quad on a scale. + * + * @unit kg + * @min 0.0 + * @decimal 2 + * @increment 0.1 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_MASS, 1.0f); + +/** + * Vehicle inertia about X axis + * + * The intertia is a 3 by 3 symmetric matrix. + * It represents the difficulty of the vehicle to modify its angular rate. + * + * @unit kg m^2 + * @min 0.0 + * @decimal 3 + * @increment 0.005 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_IXX, 0.025f); + +/** + * Vehicle inertia about Y axis + * + * The intertia is a 3 by 3 symmetric matrix. + * It represents the difficulty of the vehicle to modify its angular rate. + * + * @unit kg m^2 + * @min 0.0 + * @decimal 3 + * @increment 0.005 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_IYY, 0.025f); + +/** + * Vehicle inertia about Z axis + * + * The intertia is a 3 by 3 symmetric matrix. + * It represents the difficulty of the vehicle to modify its angular rate. + * + * @unit kg m^2 + * @min 0.0 + * @decimal 3 + * @increment 0.005 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_IZZ, 0.030f); + +/** + * Vehicle cross term inertia xy + * + * The intertia is a 3 by 3 symmetric matrix. + * This value can be set to 0 for a quad symmetric about its center of mass. + * + * @unit kg m^2 + * @decimal 3 + * @increment 0.005 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_IXY, 0.0f); + +/** + * Vehicle cross term inertia xz + * + * The intertia is a 3 by 3 symmetric matrix. + * This value can be set to 0 for a quad symmetric about its center of mass. + * + * @unit kg m^2 + * @decimal 3 + * @increment 0.005 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_IXZ, 0.0f); + +/** + * Vehicle cross term inertia yz + * + * The intertia is a 3 by 3 symmetric matrix. + * This value can be set to 0 for a quad symmetric about its center of mass. + * + * @unit kg m^2 + * @decimal 3 + * @increment 0.005 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_IYZ, 0.0f); + +/** + * Max propeller thrust force + * + * This is the maximum force delivered by one propeller + * when the motor is running at full speed. + * + * This value is usually about 5 times the mass of the quadrotor. + * + * @unit N + * @min 0.0 + * @decimal 2 + * @increment 0.5 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_T_MAX, 5.0f); + +/** + * Max propeller torque + * + * This is the maximum torque delivered by one propeller + * when the motor is running at full speed. + * + * This value is usually about few percent of the maximum thrust force. + * + * @unit Nm + * @min 0.0 + * @decimal 3 + * @increment 0.05 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_Q_MAX, 0.1f); + +/** + * Roll arm length + * + * This is the arm length generating the rolling moment + * + * This value can be measured with a ruler. + * This corresponds to half the distance between the left and right motors. + * + * @unit m + * @min 0.0 + * @decimal 2 + * @increment 0.05 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_L_ROLL, 0.2f); + +/** + * Pitch arm length + * + * This is the arm length generating the pitching moment + * + * This value can be measured with a ruler. + * This corresponds to half the distance between the front and rear motors. + * + * @unit m + * @min 0.0 + * @decimal 2 + * @increment 0.05 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_L_PITCH, 0.2f); + +/** + * First order drag coefficient + * + * Physical coefficient representing the friction with air particules. + * The greater this value, the slower the quad will move. + * + * Drag force function of velocity: D=-KDV*V. + * The maximum freefall velocity can be computed as V=10*MASS/KDV [m/s] + * + * @unit N/(m/s) + * @min 0.0 + * @decimal 2 + * @increment 0.05 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_KDV, 1.0f); + +/** + * First order angular damper coefficient + * + * Physical coefficient representing the friction with air particules during rotations. + * The greater this value, the slower the quad will rotate. + * + * Aerodynamic moment function of body rate: Ma=-KDW*W_B. + * This value can be set to 0 if unknown. + * + * @unit Nm/(rad/s) + * @min 0.0 + * @decimal 3 + * @increment 0.005 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f); + +/** + * Initial geodetic latitude + * + * This value represents the North-South location on Earth where the simulation begins. + * A value of 45 deg should be written 450000000. + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit deg*1e7 + * @min -850000000 + * @max 850000000 + * @group Simulation In Hardware + */ +PARAM_DEFINE_INT32(SIH_LOC_LAT0, 454671160); + +/** + * Initial geodetic longitude + * + * This value represents the East-West location on Earth where the simulation begins. + * A value of 45 deg should be written 450000000. + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit deg*1e7 + * @min -1800000000 + * @max 1800000000 + * @group Simulation In Hardware + */ +PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370); + +/** + * Initial AMSL ground altitude + * + * This value represents the Above Mean Sea Level (AMSL) altitude where the simulation begins. + * + * If using FlightGear as a visual animation, + * this value can be tweaked such that the vehicle lies on the ground at takeoff. + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * + * @unit m + * @min -420.0 + * @max 8848.0 + * @decimal 2 + * @increment 0.01 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_LOC_H0, 32.34f); + +/** + * North magnetic field at the initial location + * + * This value represents the North magnetic field at the initial location. + * + * A magnetic field calculator can be found on the NOAA website + * Note, the values need to be converted from nano Tesla to Gauss + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit gauss + * @min -1.0 + * @max 1.0 + * @decimal 2 + * @increment 0.001 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_LOC_MU_X, 0.179f); + +/** + * East magnetic field at the initial location + * + * This value represents the East magnetic field at the initial location. + * + * A magnetic field calculator can be found on the NOAA website + * Note, the values need to be converted from nano Tesla to Gauss + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit gauss + * @min -1.0 + * @max 1.0 + * @decimal 2 + * @increment 0.001 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f); + +/** + * Down magnetic field at the initial location + * + * This value represents the Down magnetic field at the initial location. + * + * A magnetic field calculator can be found on the NOAA website + * Note, the values need to be converted from nano Tesla to Gauss + * + * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others + * to represent a physical ground location on Earth. + * + * @unit gauss + * @min -1.0 + * @max 1.0 + * @decimal 2 + * @increment 0.001 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z, 0.504f); + +/** + * Number of GPS satellites used + * + * @min 0 + * @max 50 + * @group Simulation In Hardware + */ +PARAM_DEFINE_INT32(SIH_GPS_USED, 10); + +/** + * Barometer offset in meters + * + * Absolute value superior to 10000 will disable barometer + * + * @unit m + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_BARO_OFFSET, 0.0f); + +/** + * magnetometer X offset in Gauss + * + * Absolute value superior to 10000 will disable magnetometer + * + * @unit gauss + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_X, 0.0f); + +/** + * magnetometer Y offset in Gauss + * + * Absolute value superior to 10000 will disable magnetometer + * + * @unit gauss + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Y, 0.0f); +/** + * magnetometer Z offset in Gauss + * + * Absolute value superior to 10000 will disable magnetometer + * + * @unit gauss + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Z, 0.0f); + +/** + * distance sensor minimun range + * + * @unit m + * @min 0.0 + * @max 10.0 + * @decimal 4 + * @increment 0.01 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f); + +/** + * distance sensor maximun range + * + * @unit m + * @min 0.0 + * @max 1000.0 + * @decimal 4 + * @increment 0.01 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f); + +/** + * if >= 0 the distance sensor measures will be overrided by this value + * + * Absolute value superior to 10000 will disable distance sensor + * + * @unit m + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_DISTSNSR_OVR, -1.0f); + +/** + * thruster time constant tau + * + * the time taken for the thruster to step from 0 to 100% should be about 4 times tau + * + * @unit s + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); diff --git a/src/prometheus_px4/src/modules/simulator/CMakeLists.txt b/src/prometheus_px4/src/modules/simulator/CMakeLists.txt new file mode 100644 index 0000000..fdc6837 --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/CMakeLists.txt @@ -0,0 +1,79 @@ +############################################################################ +# +# 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. +# +############################################################################ + +option(ENABLE_UART_RC_INPUT "Enable RC Input from UART mavlink connection" OFF) + +if(ENABLE_UART_RC_INPUT) + if (APPLE) + set(PIXHAWK_DEVICE "/dev/cu.usbmodem1") + elseif (UNIX AND NOT APPLE) + set(PIXHAWK_DEVICE "/dev/ttyACM0") + elseif (WIN32) + set(PIXHAWK_DEVICE "COM3") + endif() + + set(PIXHAWK_DEVICE_BAUD 115200) +endif() +configure_file(simulator_config.h.in simulator_config.h @ONLY) +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + +set(SIMULATOR_SRCS simulator.cpp) +if (NOT ${PX4_PLATFORM} STREQUAL "qurt") + list(APPEND SIMULATOR_SRCS + simulator_mavlink.cpp) +endif() + +px4_add_module( + MODULE modules__simulator + MAIN simulator + COMPILE_FLAGS + -Wno-double-promotion + -Wno-cast-align + -Wno-address-of-packed-member # TODO: fix in c_library_v2 + INCLUDES + ${PX4_SOURCE_DIR}/mavlink/include/mavlink + SRCS + ${SIMULATOR_SRCS} + DEPENDS + git_mavlink_v2 + conversion + git_ecl + ecl_geo + drivers_accelerometer + drivers_barometer + drivers_gyroscope + drivers_magnetometer + ) +target_include_directories(modules__simulator INTERFACE ${PX4_SOURCE_DIR}/mavlink/include/mavlink) + +add_subdirectory(battery_simulator) diff --git a/src/prometheus_px4/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/prometheus_px4/src/modules/simulator/battery_simulator/BatterySimulator.cpp new file mode 100644 index 0000000..4954696 --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * 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 "BatterySimulator.hpp" + +BatterySimulator::BatterySimulator() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US) +{ +} + +BatterySimulator::~BatterySimulator() +{ + perf_free(_loop_perf); +} + +bool BatterySimulator::init() +{ + ScheduleOnInterval(BATTERY_SIMLATOR_SAMPLE_INTERVAL_US); + return true; +} + +void BatterySimulator::Run() +{ + if (should_exit()) { + ScheduleClear(); + 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(¶m_update); + + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + } + + const hrt_abstime now_us = hrt_absolute_time(); + + const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000; + + if (_armed) { + if (_last_integration_us != 0) { + _battery_percentage -= (now_us - _last_integration_us) / discharge_interval_us; + } + + _last_integration_us = now_us; + + } else { + _last_integration_us = 0; + } + + float ibatt = -1.0f; // no current sensor in simulation + + _battery_percentage = math::max(_battery_percentage, _param_bat_min_pct.get() / 100.f); + float vbatt = math::gradual(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); + vbatt *= _battery.cell_count(); + + _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0.f); + + perf_end(_loop_perf); +} + +int BatterySimulator::task_spawn(int argc, char *argv[]) +{ + BatterySimulator *instance = new BatterySimulator(); + + 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 BatterySimulator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int BatterySimulator::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("battery_simulator", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int battery_simulator_main(int argc, char *argv[]) +{ + return BatterySimulator::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/simulator/battery_simulator/BatterySimulator.hpp b/src/prometheus_px4/src/modules/simulator/battery_simulator/BatterySimulator.hpp new file mode 100644 index 0000000..8adf69b --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/battery_simulator/BatterySimulator.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class BatterySimulator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + BatterySimulator(); + ~BatterySimulator() 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; + + static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz + static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ; + + uORB::Publication _battery_pub{ORB_ID(battery_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + Battery _battery; + + uint64_t _last_integration_us{0}; + float _battery_percentage{1.f}; + bool _armed{false}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sim_bat_drain, ///< battery drain interval + (ParamFloat) _param_bat_min_pct //< minimum battery percentage + ) +}; diff --git a/src/prometheus_px4/src/modules/simulator/battery_simulator/CMakeLists.txt b/src/prometheus_px4/src/modules/simulator/battery_simulator/CMakeLists.txt new file mode 100644 index 0000000..c4ba813 --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/battery_simulator/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__simulator__battery_simulator + MAIN battery_simulator + COMPILE_FLAGS + SRCS + BatterySimulator.cpp + BatterySimulator.hpp + DEPENDS + battery + mathlib + px4_work_queue + ) diff --git a/src/prometheus_px4/src/modules/simulator/battery_simulator/battery_simulator_params.c b/src/prometheus_px4/src/modules/simulator/battery_simulator/battery_simulator_params.c new file mode 100644 index 0000000..d634714 --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/battery_simulator/battery_simulator_params.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Simulator Battery drain interval + * + * @min 1 + * @max 86400 + * @increment 1 + * @unit s + * + * @group SITL + */ +PARAM_DEFINE_FLOAT(SIM_BAT_DRAIN, 60); + +/** + * Simulator Battery minimal percentage. + * + * Can be used to alter the battery level during SITL- or HITL-simulation on the fly. + * Particularly useful for testing different low-battery behaviour. + * + * @min 0 + * @max 100 + * @increment 0.1 + * @unit % + * + * @group SITL + */ +PARAM_DEFINE_FLOAT(SIM_BAT_MIN_PCT, 50.0f); diff --git a/src/prometheus_px4/src/modules/simulator/simulator.cpp b/src/prometheus_px4/src/modules/simulator/simulator.cpp new file mode 100644 index 0000000..cd9bb94 --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/simulator.cpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * Copyright (c) 2016-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 simulator.cpp + * + * This module interfaces via MAVLink to a software in the loop simulator (SITL) + * such as jMAVSim or Gazebo. + */ + +#include +#include +#include +#include +#include + +#include "simulator.h" + +static px4_task_t g_sim_task = -1; + +Simulator *Simulator::_instance = nullptr; + +void Simulator::parameters_update(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(); + } +} + +int Simulator::start(int argc, char *argv[]) +{ + _instance = new Simulator(); + + if (_instance) { + + if (argc == 5 && strcmp(argv[3], "-u") == 0) { + _instance->set_ip(InternetProtocol::UDP); + _instance->set_port(atoi(argv[4])); + } + + if (argc == 5 && strcmp(argv[3], "-c") == 0) { + _instance->set_ip(InternetProtocol::TCP); + _instance->set_port(atoi(argv[4])); + } + + if (argc == 6 && strcmp(argv[3], "-t") == 0) { + PX4_INFO("Simulator using TCP on remote host %s port %s", argv[4], argv[5]); + PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]); + _instance->set_ip(InternetProtocol::TCP); + _instance->set_tcp_remote_ipaddr(argv[4]); + _instance->set_port(atoi(argv[5])); + } + + if (argc == 6 && strcmp(argv[3], "-h") == 0) { + PX4_INFO("Simulator using TCP on remote host %s port %s", argv[4], argv[5]); + PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]); + _instance->set_ip(InternetProtocol::TCP); + _instance->set_hostname(argv[4]); + _instance->set_port(atoi(argv[5])); + } + + _instance->run(); + + return 0; + + } else { + PX4_WARN("Simulator creation failed"); + return 1; + } +} + +static void usage() +{ + PX4_INFO("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop|status}"); + PX4_INFO("Start simulator: simulator start"); + PX4_INFO("Connect using UDP: simulator start -u udp_port"); + PX4_INFO("Connect using TCP: simulator start -c tcp_port"); + PX4_INFO("Connect to a remote server using TCP: simulator start -t ip_addr tcp_port"); + PX4_INFO("Connect to a remote server via hostname using TCP: simulator start -h hostname tcp_port"); +} + +__BEGIN_DECLS +extern int simulator_main(int argc, char *argv[]); +__END_DECLS + + +int simulator_main(int argc, char *argv[]) +{ + if (argc > 1 && strcmp(argv[1], "start") == 0) { + + if (g_sim_task >= 0) { + PX4_WARN("Simulator already started"); + return 0; + } + + g_sim_task = px4_task_spawn_cmd("simulator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1500, + Simulator::start, + argv); + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + + // We want to prevent the rest of the startup script from running until time + // is initialized by the HIL_SENSOR messages from the simulator. + while (true) { + if (Simulator::getInstance() && Simulator::getInstance()->has_initialized()) { + break; + } + + system_usleep(100); + } + +#endif + + } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_sim_task < 0) { + PX4_WARN("Simulator not running"); + return 1; + + } else { + px4_task_delete(g_sim_task); + g_sim_task = -1; + } + + } else if (argc == 2 && strcmp(argv[1], "status") == 0) { + if (g_sim_task < 0) { + PX4_WARN("Simulator not running"); + return 1; + + } else { + PX4_INFO("running"); + } + + } else { + usage(); + return 1; + } + + return 0; +} diff --git a/src/prometheus_px4/src/modules/simulator/simulator.h b/src/prometheus_px4/src/modules/simulator/simulator.h new file mode 100644 index 0000000..7de8eb6 --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/simulator.h @@ -0,0 +1,306 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * Copyright (c) 2016-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 simulator.h + * + * This module interfaces via MAVLink to a software in the loop simulator (SITL) + * such as jMAVSim or Gazebo. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace time_literals; + +//! Enumeration to use on the bitmask in HIL_SENSOR +enum class SensorSource { + ACCEL = 0b111, + GYRO = 0b111000, + MAG = 0b111000000, + BARO = 0b1101000000000, + DIFF_PRESS = 0b10000000000 +}; +ENABLE_BIT_OPERATORS(SensorSource) + +//! AND operation for the enumeration and unsigned types that returns the bitmask +template +static inline SensorSource operator &(A lhs, B rhs) +{ + // make it type safe + static_assert((std::is_same::value || std::is_same::value), + "first argument is not uint32_t or SensorSource enum type"); + static_assert((std::is_same::value || std::is_same::value), + "second argument is not uint32_t or SensorSource enum type"); + + typedef typename std::underlying_type::type underlying; + + return static_cast( + static_cast(lhs) & + static_cast(rhs) + ); +} + +class Simulator : public ModuleParams +{ +public: + static Simulator *getInstance() { return _instance; } + + enum class InternetProtocol { + TCP, + UDP + }; + + static int start(int argc, char *argv[]); + + void set_ip(InternetProtocol ip) { _ip = ip; } + void set_port(unsigned port) { _port = port; } + void set_hostname(std::string hostname) { _hostname = hostname; } + void set_tcp_remote_ipaddr(char *tcp_remote_ipaddr) { _tcp_remote_ipaddr = tcp_remote_ipaddr; } + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + bool has_initialized() { return _has_initialized.load(); } +#endif + +private: + Simulator() : ModuleParams(nullptr) + { + } + + ~Simulator() + { + // free perf counters + perf_free(_perf_sim_delay); + perf_free(_perf_sim_interval); + + for (size_t i = 0; i < sizeof(_dist_pubs) / sizeof(_dist_pubs[0]); i++) { + delete _dist_pubs[i]; + } + + px4_lockstep_unregister_component(_lockstep_component); + + for (size_t i = 0; i < sizeof(_sensor_gps_pubs) / sizeof(_sensor_gps_pubs[0]); i++) { + delete _sensor_gps_pubs[i]; + } + + _instance = nullptr; + } + + + void check_failure_injections(); + + int publish_flow_topic(const mavlink_hil_optical_flow_t *flow); + int publish_odometry_topic(const mavlink_message_t *odom_mavlink); + int publish_distance_topic(const mavlink_distance_sensor_t *dist); + + static Simulator *_instance; + + // simulated sensor instances + static constexpr uint8_t ACCEL_COUNT_MAX = 3; + PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] { + {1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + {1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION + {1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + }; + + static constexpr uint8_t GYRO_COUNT_MAX = 3; + PX4Gyroscope _px4_gyro[GYRO_COUNT_MAX] { + {1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + {1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION + {1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + }; + + PX4Magnetometer _px4_mag_0{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Magnetometer _px4_mag_1{197644, ROTATION_NONE}; // 197644: DRV_MAG_DEVTYPE_MAGSIM, BUS: 2, ADDR: 1, TYPE: SIMULATION + + PX4Barometer _px4_baro_0{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + PX4Barometer _px4_baro_1{6620428}; // 6620428: DRV_BARO_DEVTYPE_BAROSIM, BUS: 2, ADDR: 4, TYPE: SIMULATION + + float _sensors_temperature{0}; + + perf_counter_t _perf_sim_delay{perf_alloc(PC_ELAPSED, MODULE_NAME": network delay")}; + perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")}; + + // uORB publisher handlers + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; + uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; + uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; + + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + uORB::PublicationMulti *_dist_pubs[ORB_MULTI_MAX_INSTANCES] {}; + uint32_t _dist_sensor_ids[ORB_MULTI_MAX_INSTANCES] {}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + unsigned int _port{14560}; + + InternetProtocol _ip{InternetProtocol::UDP}; + + std::string _hostname{""}; + + char *_tcp_remote_ipaddr{nullptr}; + + double _realtime_factor{1.0}; ///< How fast the simulation runs in comparison to real system time + + hrt_abstime _last_sim_timestamp{0}; + hrt_abstime _last_sitl_timestamp{0}; + + + void run(); + void handle_message(const mavlink_message_t *msg); + void handle_message_distance_sensor(const mavlink_message_t *msg); + void handle_message_hil_gps(const mavlink_message_t *msg); + void handle_message_hil_sensor(const mavlink_message_t *msg); + void handle_message_hil_state_quaternion(const mavlink_message_t *msg); + void handle_message_landing_target(const mavlink_message_t *msg); + void handle_message_odometry(const mavlink_message_t *msg); + void handle_message_optical_flow(const mavlink_message_t *msg); + void handle_message_rc_channels(const mavlink_message_t *msg); + void handle_message_vision_position_estimate(const mavlink_message_t *msg); + + void parameters_update(bool force); + void poll_for_MAVLink_messages(); + void request_hil_state_quaternion(); + void send(); + void send_controls(); + void send_heartbeat(); + void send_mavlink_message(const mavlink_message_t &aMsg); + void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors); + + static void *sending_trampoline(void *); + + void actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg); + + + // uORB publisher handlers + uORB::Publication _vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; + uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; + uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; + uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; + uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; + + // HIL GPS + static constexpr int MAX_GPS = 3; + uORB::PublicationMulti *_sensor_gps_pubs[MAX_GPS] {}; + uint8_t _gps_ids[MAX_GPS] {}; + std::default_random_engine _gen{}; + + // uORB subscription handlers + int _actuator_outputs_sub{-1}; + actuator_outputs_s _actuator_outputs{}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + + // hil map_ref data + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + + vehicle_status_s _vehicle_status{}; + + bool _accel_blocked[ACCEL_COUNT_MAX] {}; + bool _accel_stuck[ACCEL_COUNT_MAX] {}; + sensor_accel_fifo_s _last_accel_fifo{}; + matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {}; + + bool _gyro_blocked[GYRO_COUNT_MAX] {}; + bool _gyro_stuck[GYRO_COUNT_MAX] {}; + sensor_gyro_fifo_s _last_gyro_fifo{}; + matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {}; + + bool _baro_blocked{false}; + bool _baro_stuck{false}; + + bool _mag_blocked{false}; + bool _mag_stuck{false}; + + bool _gps_blocked{false}; + bool _airspeed_blocked{false}; + + float _last_magx{0.0f}; + float _last_magy{0.0f}; + float _last_magz{0.0f}; + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + px4::atomic _has_initialized {false}; +#endif + + int _lockstep_component{-1}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mav_type, + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_comp_id + ) +}; diff --git a/src/prometheus_px4/src/modules/simulator/simulator_config.h.in b/src/prometheus_px4/src/modules/simulator/simulator_config.h.in new file mode 100644 index 0000000..a60b491 --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/simulator_config.h.in @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (c) 2016 Anton Matosov. 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 + +#cmakedefine ENABLE_UART_RC_INPUT + +#cmakedefine PIXHAWK_DEVICE "@PIXHAWK_DEVICE@" +#cmakedefine PIXHAWK_DEVICE_BAUD @PIXHAWK_DEVICE_BAUD@ diff --git a/src/prometheus_px4/src/modules/simulator/simulator_mavlink.cpp b/src/prometheus_px4/src/modules/simulator/simulator_mavlink.cpp new file mode 100644 index 0000000..1dc01db --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/simulator_mavlink.cpp @@ -0,0 +1,1482 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * Copyright (c) 2016 Anton Matosov. All rights reserved. + * Copyright (c) 2017-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 "simulator.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef ENABLE_UART_RC_INPUT +#ifndef B460800 +#define B460800 460800 +#endif + +#ifndef B921600 +#define B921600 921600 +#endif + +static int openUart(const char *uart_name, int baud); +#endif + +static int _fd; +static unsigned char _buf[2048]; +static sockaddr_in _srcaddr; +static unsigned _addrlen = sizeof(_srcaddr); + +const unsigned mode_flag_armed = 128; +const unsigned mode_flag_custom = 1; + +using namespace time_literals; + +void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg) +{ + memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t)); + + msg->time_usec = hrt_absolute_time() + hrt_absolute_time_offset(); + + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + int _system_type = _param_mav_type.get(); + + /* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust) + all other motors are configured for -1..1 range */ + unsigned pos_thrust_motors_count; + bool is_fixed_wing; + + switch (_system_type) { + case MAV_TYPE_AIRSHIP: + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_COAXIAL: + pos_thrust_motors_count = 2; + is_fixed_wing = false; + break; + + case MAV_TYPE_TRICOPTER: + pos_thrust_motors_count = 3; + is_fixed_wing = false; + break; + + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + pos_thrust_motors_count = 4; + is_fixed_wing = false; + break; + + case MAV_TYPE_VTOL_RESERVED2: + pos_thrust_motors_count = 5; + is_fixed_wing = false; + break; + + case MAV_TYPE_HEXAROTOR: + pos_thrust_motors_count = 6; + is_fixed_wing = false; + break; + + case MAV_TYPE_OCTOROTOR: + pos_thrust_motors_count = 8; + is_fixed_wing = false; + break; + + case MAV_TYPE_SUBMARINE: + pos_thrust_motors_count = 0; + is_fixed_wing = false; + break; + + case MAV_TYPE_FIXED_WING: + pos_thrust_motors_count = 0; + is_fixed_wing = true; + break; + + default: + pos_thrust_motors_count = 0; + is_fixed_wing = false; + break; + } + + for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + if (!armed) { + /* send 0 when disarmed and for disabled channels */ + msg->controls[i] = 0.0f; + + } else if ((is_fixed_wing && i == 4) || + (!is_fixed_wing && i < pos_thrust_motors_count)) { //multirotor, rotor channel + /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ + msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); + msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f); + + } else { + const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; + const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2; + + /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */ + msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta; + msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f); + } + + } + + msg->mode = mode_flag_custom; + msg->mode |= (armed) ? mode_flag_armed : 0; + msg->flags = 0; + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + msg->flags |= 1; +#endif +} + +void Simulator::send_controls() +{ + orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs); + + if (_actuator_outputs.timestamp > 0) { + mavlink_hil_actuator_controls_t hil_act_control; + actuator_controls_from_outputs(&hil_act_control); + + mavlink_message_t message{}; + mavlink_msg_hil_actuator_controls_encode(_param_mav_sys_id.get(), _param_mav_comp_id.get(), &message, &hil_act_control); + + PX4_DEBUG("sending controls t=%ld (%ld)", _actuator_outputs.timestamp, hil_act_control.time_usec); + + send_mavlink_message(message); + } +} + +void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors) +{ + // temperature only updated with baro + if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO) { + if (PX4_ISFINITE(sensors.temperature)) { + _px4_mag_0.set_temperature(sensors.temperature); + _px4_mag_1.set_temperature(sensors.temperature); + + _sensors_temperature = sensors.temperature; + } + } + + // accel + if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) { + for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + if (i == 0) { + // accel 0 is simulated FIFO + static constexpr float ACCEL_FIFO_SCALE = CONSTANTS_ONE_G / 2048.f; + static constexpr float ACCEL_FIFO_RANGE = 16.f * CONSTANTS_ONE_G; + + _px4_accel[i].set_scale(ACCEL_FIFO_SCALE); + _px4_accel[i].set_range(ACCEL_FIFO_RANGE); + + if (_accel_stuck[i]) { + _px4_accel[i].updateFIFO(_last_accel_fifo); + + } else if (!_accel_blocked[i]) { + _px4_accel[i].set_temperature(_sensors_temperature); + + _last_accel_fifo.samples = 1; + _last_accel_fifo.dt = time - _last_accel_fifo.timestamp_sample; + _last_accel_fifo.timestamp_sample = time; + _last_accel_fifo.x[0] = sensors.xacc / ACCEL_FIFO_SCALE; + _last_accel_fifo.y[0] = sensors.yacc / ACCEL_FIFO_SCALE; + _last_accel_fifo.z[0] = sensors.zacc / ACCEL_FIFO_SCALE; + + _px4_accel[i].updateFIFO(_last_accel_fifo); + } + + } else { + if (_accel_stuck[i]) { + _px4_accel[i].update(time, _last_accel[i](0), _last_accel[i](1), _last_accel[i](2)); + + } else if (!_accel_blocked[i]) { + _px4_accel[i].set_temperature(_sensors_temperature); + _px4_accel[i].update(time, sensors.xacc, sensors.yacc, sensors.zacc); + _last_accel[i] = matrix::Vector3f{sensors.xacc, sensors.yacc, sensors.zacc}; + } + } + } + } + + // gyro + if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) { + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + if (i == 0) { + // gyro 0 is simulated FIFO + static constexpr float GYRO_FIFO_SCALE = math::radians(2000.f / 32768.f); + static constexpr float GYRO_FIFO_RANGE = math::radians(2000.f); + + _px4_gyro[i].set_scale(GYRO_FIFO_SCALE); + _px4_gyro[i].set_range(GYRO_FIFO_RANGE); + + if (_gyro_stuck[i]) { + _px4_gyro[i].updateFIFO(_last_gyro_fifo); + + } else if (!_gyro_blocked[i]) { + _px4_gyro[i].set_temperature(_sensors_temperature); + + _last_gyro_fifo.samples = 1; + _last_gyro_fifo.dt = time - _last_gyro_fifo.timestamp_sample; + _last_gyro_fifo.timestamp_sample = time; + _last_gyro_fifo.x[0] = sensors.xgyro / GYRO_FIFO_SCALE; + _last_gyro_fifo.y[0] = sensors.ygyro / GYRO_FIFO_SCALE; + _last_gyro_fifo.z[0] = sensors.zgyro / GYRO_FIFO_SCALE; + + _px4_gyro[i].updateFIFO(_last_gyro_fifo); + } + + } else { + if (_gyro_stuck[i]) { + _px4_gyro[i].update(time, _last_gyro[i](0), _last_gyro[i](1), _last_gyro[i](2)); + + } else if (!_gyro_blocked[i]) { + _px4_gyro[i].set_temperature(_sensors_temperature); + _px4_gyro[i].update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro); + _last_gyro[i] = matrix::Vector3f{sensors.xgyro, sensors.ygyro, sensors.zgyro}; + } + } + } + } + + // magnetometer + if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_mag_blocked) { + if (_mag_stuck) { + _px4_mag_0.update(time, _last_magx, _last_magy, _last_magz); + _px4_mag_1.update(time, _last_magx, _last_magy, _last_magz); + + } else { + _px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag); + _px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag); + _last_magx = sensors.xmag; + _last_magy = sensors.ymag; + _last_magz = sensors.zmag; + } + } + + // baro + if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) { + if (_baro_stuck) { + _px4_baro_0.update(time, _px4_baro_0.get().pressure); + _px4_baro_0.set_temperature(_px4_baro_0.get().temperature); + _px4_baro_1.update(time, _px4_baro_1.get().pressure); + _px4_baro_1.set_temperature(_px4_baro_1.get().temperature); + + } else { + _px4_baro_0.update(time, sensors.abs_pressure); + _px4_baro_0.set_temperature(sensors.temperature); + _px4_baro_1.update(time, sensors.abs_pressure); + _px4_baro_1.set_temperature(sensors.temperature); + } + } + + // differential pressure + if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) { + differential_pressure_s report{}; + report.timestamp = time; + report.temperature = _sensors_temperature; + report.differential_pressure_filtered_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar; + report.differential_pressure_raw_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar; + + _differential_pressure_pub.publish(report); + } +} + +void Simulator::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor(msg); + break; + + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_optical_flow(msg); + break; + + case MAVLINK_MSG_ID_ODOMETRY: + handle_message_odometry(msg); + break; + + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); + break; + + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + handle_message_distance_sensor(msg); + break; + + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; + + case MAVLINK_MSG_ID_RC_CHANNELS: + handle_message_rc_channels(msg); + break; + + case MAVLINK_MSG_ID_LANDING_TARGET: + handle_message_landing_target(msg); + break; + + case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: + handle_message_hil_state_quaternion(msg); + break; + } +} + +void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg) +{ + mavlink_distance_sensor_t dist; + mavlink_msg_distance_sensor_decode(msg, &dist); + publish_distance_topic(&dist); +} + +void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) +{ + mavlink_hil_gps_t hil_gps; + mavlink_msg_hil_gps_decode(msg, &hil_gps); + + if (!_gps_blocked) { + sensor_gps_s gps{}; + + gps.lat = hil_gps.lat; + gps.lon = hil_gps.lon; + gps.alt = hil_gps.alt; + gps.alt_ellipsoid = hil_gps.alt; + + gps.s_variance_m_s = 0.25f; + gps.c_variance_rad = 0.5f; + gps.fix_type = hil_gps.fix_type; + + gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m + gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m + + gps.hdop = 0; // TODO + gps.vdop = 0; // TODO + + gps.noise_per_ms = 0; + gps.automatic_gain_control = 0; + gps.jamming_indicator = 0; + gps.jamming_state = 0; + + gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s + gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s + gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s + gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s + gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad + gps.vel_ned_valid = true; + + gps.timestamp_time_relative = 0; + gps.time_utc_usec = hil_gps.time_usec; + + gps.satellites_used = hil_gps.satellites_visible; + + gps.heading = NAN; + gps.heading_offset = NAN; + + gps.timestamp = hrt_absolute_time(); + + // New publishers will be created based on the HIL_GPS ID's being different or not + for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) { + if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) { + _sensor_gps_pubs[i]->publish(gps); + break; + } + + if (_sensor_gps_pubs[i] == nullptr) { + _sensor_gps_pubs[i] = new uORB::PublicationMulti {ORB_ID(sensor_gps)}; + _gps_ids[i] = hil_gps.id; + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = i; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + gps.device_id = device_id.devid; + + _sensor_gps_pubs[i]->publish(gps); + break; + } + } + } +} + +void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) +{ + if (_lockstep_component == -1) { + _lockstep_component = px4_lockstep_register_component(); + } + + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); + + struct timespec ts; + abstime_to_ts(&ts, imu.time_usec); + px4_clock_settime(CLOCK_MONOTONIC, &ts); + + hrt_abstime now_us = hrt_absolute_time(); + +#if 0 + // This is just for to debug missing HIL_SENSOR messages. + static hrt_abstime last_time = 0; + hrt_abstime diff = now_us - last_time; + float step = diff / 4000.0f; + + if (step > 1.1f || step < 0.9f) { + PX4_INFO("HIL_SENSOR: imu time_usec: %lu, time_usec: %lu, diff: %lu, step: %.2f", imu.time_usec, now_us, diff, step); + } + + last_time = now_us; +#endif + + update_sensors(now_us, imu); + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + + if (!_has_initialized.load()) { + _has_initialized.store(true); + } + +#endif + + px4_lockstep_progress(_lockstep_component); +} + +void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg) +{ + mavlink_hil_state_quaternion_t hil_state; + mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); + + uint64_t timestamp = hrt_absolute_time(); + + /* angular velocity */ + vehicle_angular_velocity_s hil_angular_velocity{}; + { + hil_angular_velocity.timestamp = timestamp; + + hil_angular_velocity.xyz[0] = hil_state.rollspeed; + hil_angular_velocity.xyz[1] = hil_state.pitchspeed; + hil_angular_velocity.xyz[2] = hil_state.yawspeed; + + // always publish ground truth attitude message + _vehicle_angular_velocity_ground_truth_pub.publish(hil_angular_velocity); + } + + /* attitude */ + vehicle_attitude_s hil_attitude{}; + { + hil_attitude.timestamp = timestamp; + + matrix::Quatf q(hil_state.attitude_quaternion); + q.copyTo(hil_attitude.q); + + // always publish ground truth attitude message + _attitude_ground_truth_pub.publish(hil_attitude); + } + + /* global position */ + vehicle_global_position_s hil_gpos{}; + { + hil_gpos.timestamp = timestamp; + + hil_gpos.lat = hil_state.lat / 1E7;//1E7 + hil_gpos.lon = hil_state.lon / 1E7;//1E7 + hil_gpos.alt = hil_state.alt / 1E3;//1E3 + + // always publish ground truth attitude message + _gpos_ground_truth_pub.publish(hil_gpos); + } + + /* local position */ + vehicle_local_position_s hil_lpos{}; + { + hil_lpos.timestamp = timestamp; + + double lat = hil_state.lat * 1e-7; + double lon = hil_state.lon * 1e-7; + + if (!map_projection_initialized(&_global_local_proj_ref)) { + map_projection_init(&_global_local_proj_ref, lat, lon); + _global_local_alt0 = hil_state.alt / 1000.f; + } + + float x; + float y; + map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y); + hil_lpos.timestamp = timestamp; + hil_lpos.xy_valid = true; + hil_lpos.z_valid = true; + hil_lpos.v_xy_valid = true; + hil_lpos.v_z_valid = true; + hil_lpos.x = x; + hil_lpos.y = y; + hil_lpos.z = _global_local_alt0 - hil_state.alt / 1000.0f; + hil_lpos.vx = hil_state.vx / 100.0f; + hil_lpos.vy = hil_state.vy / 100.0f; + hil_lpos.vz = hil_state.vz / 100.0f; + matrix::Eulerf euler = matrix::Quatf(hil_attitude.q); + hil_lpos.heading = euler.psi(); + hil_lpos.xy_global = true; + hil_lpos.z_global = true; + hil_lpos.ref_timestamp = _global_local_proj_ref.timestamp; + hil_lpos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad); + hil_lpos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad); + hil_lpos.ref_alt = _global_local_alt0; + hil_lpos.vxy_max = std::numeric_limits::infinity(); + hil_lpos.vz_max = std::numeric_limits::infinity(); + hil_lpos.hagl_min = std::numeric_limits::infinity(); + hil_lpos.hagl_max = std::numeric_limits::infinity(); + + // always publish ground truth attitude message + _lpos_ground_truth_pub.publish(hil_lpos); + } +} + +void Simulator::handle_message_landing_target(const mavlink_message_t *msg) +{ + mavlink_landing_target_t landing_target_mavlink; + mavlink_msg_landing_target_decode(msg, &landing_target_mavlink); + + if (landing_target_mavlink.position_valid) { + PX4_WARN("Only landing targets relative to captured images are supported"); + + } else { + irlock_report_s report{}; + report.timestamp = hrt_absolute_time(); + report.signature = landing_target_mavlink.target_num; + report.pos_x = landing_target_mavlink.angle_x; + report.pos_y = landing_target_mavlink.angle_y; + report.size_x = landing_target_mavlink.size_x; + report.size_y = landing_target_mavlink.size_y; + + _irlock_report_pub.publish(report); + } +} + +void Simulator::handle_message_odometry(const mavlink_message_t *msg) +{ + publish_odometry_topic(msg); +} + +void Simulator::handle_message_optical_flow(const mavlink_message_t *msg) +{ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + publish_flow_topic(&flow); +} + +void Simulator::handle_message_rc_channels(const mavlink_message_t *msg) +{ + mavlink_rc_channels_t rc_channels; + mavlink_msg_rc_channels_decode(msg, &rc_channels); + + input_rc_s rc_input{}; + rc_input.timestamp_last_signal = hrt_absolute_time(); + rc_input.channel_count = rc_channels.chancount; + rc_input.rssi = rc_channels.rssi; + rc_input.values[0] = rc_channels.chan1_raw; + rc_input.values[1] = rc_channels.chan2_raw; + rc_input.values[2] = rc_channels.chan3_raw; + rc_input.values[3] = rc_channels.chan4_raw; + rc_input.values[4] = rc_channels.chan5_raw; + rc_input.values[5] = rc_channels.chan6_raw; + rc_input.values[6] = rc_channels.chan7_raw; + rc_input.values[7] = rc_channels.chan8_raw; + rc_input.values[8] = rc_channels.chan9_raw; + rc_input.values[9] = rc_channels.chan10_raw; + rc_input.values[10] = rc_channels.chan11_raw; + rc_input.values[11] = rc_channels.chan12_raw; + rc_input.values[12] = rc_channels.chan13_raw; + rc_input.values[13] = rc_channels.chan14_raw; + rc_input.values[14] = rc_channels.chan15_raw; + rc_input.values[15] = rc_channels.chan16_raw; + rc_input.values[16] = rc_channels.chan17_raw; + rc_input.values[17] = rc_channels.chan18_raw; + + rc_input.timestamp = hrt_absolute_time(); + + // publish message + _input_rc_pub.publish(rc_input); +} + +void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg) +{ + publish_odometry_topic(msg); +} + +void Simulator::send_mavlink_message(const mavlink_message_t &aMsg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + uint16_t bufLen = 0; + + bufLen = mavlink_msg_to_send_buffer(buf, &aMsg); + + ssize_t len; + + if (_ip == InternetProtocol::UDP) { + len = ::sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr)); + + } else { + len = ::send(_fd, buf, bufLen, 0); + } + + if (len <= 0) { + PX4_WARN("Failed sending mavlink message: %s", strerror(errno)); + } +} + +void *Simulator::sending_trampoline(void * /*unused*/) +{ + _instance->send(); + return nullptr; +} + +void Simulator::send() +{ +#ifdef __PX4_DARWIN + pthread_setname_np("sim_send"); +#else + pthread_setname_np(pthread_self(), "sim_send"); +#endif + + // Subscribe to topics. + // Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS. + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + + // Before starting, we ought to send a heartbeat to initiate the SITL + // simulator to start sending sensor data which will set the time and + // get everything rolling. + // Without this, we get stuck at px4_poll which waits for a time update. + send_heartbeat(); + + px4_pollfd_struct_t fds_actuator_outputs[1] = {}; + fds_actuator_outputs[0].fd = _actuator_outputs_sub; + fds_actuator_outputs[0].events = POLLIN; + + while (true) { + + // Wait for up to 100ms for data. + int pret = px4_poll(&fds_actuator_outputs[0], 1, 100); + + if (pret == 0) { + // Timed out, try again. + continue; + } + + if (pret < 0) { + PX4_ERR("poll error %s", strerror(errno)); + continue; + } + + if (fds_actuator_outputs[0].revents & POLLIN) { + // Got new data to read, update all topics. + parameters_update(false); + check_failure_injections(); + _vehicle_status_sub.update(&_vehicle_status); + + // Wait for other modules, such as logger or ekf2 + px4_lockstep_wait_for_components(); + + send_controls(); + } + } + + orb_unsubscribe(_actuator_outputs_sub); +} + +void Simulator::request_hil_state_quaternion() +{ + mavlink_command_long_t cmd_long = {}; + mavlink_message_t message = {}; + cmd_long.command = MAV_CMD_SET_MESSAGE_INTERVAL; + cmd_long.param1 = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; + cmd_long.param2 = 5e3; + mavlink_msg_command_long_encode(_param_mav_sys_id.get(), _param_mav_comp_id.get(), &message, &cmd_long); + send_mavlink_message(message); +} + +void Simulator::send_heartbeat() +{ + mavlink_heartbeat_t hb = {}; + mavlink_message_t message = {}; + hb.autopilot = 12; + hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; + mavlink_msg_heartbeat_encode(_param_mav_sys_id.get(), _param_mav_comp_id.get(), &message, &hb); + send_mavlink_message(message); +} + +void Simulator::run() +{ +#ifdef __PX4_DARWIN + pthread_setname_np("sim_rcv"); +#else + pthread_setname_np(pthread_self(), "sim_rcv"); +#endif + + struct sockaddr_in _myaddr {}; + _myaddr.sin_family = AF_INET; + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + _myaddr.sin_port = htons(_port); + + if (_tcp_remote_ipaddr != nullptr) { + _myaddr.sin_addr.s_addr = inet_addr(_tcp_remote_ipaddr); + + } else if (!_hostname.empty()) { + /* resolve hostname */ + struct hostent *host; + host = gethostbyname(_hostname.c_str()); + memcpy(&_myaddr.sin_addr, host->h_addr_list[0], host->h_length); + + char ip[30]; + strcpy(ip, (char *)inet_ntoa((struct in_addr)_myaddr.sin_addr)); + PX4_INFO("Resolved host '%s' to address: %s", _hostname.c_str(), ip); + } + + + if (_ip == InternetProtocol::UDP) { + + if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_ERR("Creating UDP socket failed: %s", strerror(errno)); + return; + } + + if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + PX4_ERR("bind for UDP port %i failed (%i)", _port, errno); + ::close(_fd); + return; + } + + PX4_INFO("Waiting for simulator to connect on UDP port %u", _port); + + while (true) { + // Once we receive something, we're most probably good and can carry on. + int len = ::recvfrom(_fd, _buf, sizeof(_buf), 0, + (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen); + + if (len > 0) { + break; + + } else { + system_usleep(500); + } + } + + PX4_INFO("Simulator connected on UDP port %u.", _port); + + } else { + + PX4_INFO("Waiting for simulator to accept connection on TCP port %u", _port); + + while (true) { + if ((_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + PX4_ERR("Creating TCP socket failed: %s", strerror(errno)); + return; + } + + int yes = 1; + int ret = setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, (char *) &yes, sizeof(int)); + + if (ret != 0) { + PX4_ERR("setsockopt failed: %s", strerror(errno)); + } + + socklen_t myaddr_len = sizeof(_myaddr); + ret = connect(_fd, (struct sockaddr *)&_myaddr, myaddr_len); + + if (ret == 0) { + break; + + } else { + ::close(_fd); + system_usleep(500); + } + } + + PX4_INFO("Simulator connected on TCP port %u.", _port); + + } + + // Create a thread for sending data to the simulator. + pthread_t sender_thread; + + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(8000)); + + struct sched_param param; + (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); + + // sender thread should run immediately after new outputs are available + // to send the lockstep update to the simulation + param.sched_priority = SCHED_PRIORITY_ACTUATOR_OUTPUTS + 1; + (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); + + struct pollfd fds[2] = {}; + unsigned fd_count = 1; + fds[0].fd = _fd; + fds[0].events = POLLIN; + +#ifdef ENABLE_UART_RC_INPUT + // setup serial connection to autopilot (used to get manual controls) + int serial_fd = openUart(PIXHAWK_DEVICE, PIXHAWK_DEVICE_BAUD); + + char serial_buf[1024]; + + if (serial_fd >= 0) { + fds[1].fd = serial_fd; + fds[1].events = POLLIN; + fd_count++; + + PX4_INFO("Start using %s for radio control input.", PIXHAWK_DEVICE); + + } else { + PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE); + } + +#endif + + // got data from simulator, now activate the sending thread + pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr); + pthread_attr_destroy(&sender_thread_attr); + + mavlink_status_t mavlink_status = {}; + + // Request HIL_STATE_QUATERNION for ground truth. + request_hil_state_quaternion(); + + while (true) { + + // wait for new mavlink messages to arrive + int pret = ::poll(&fds[0], fd_count, 1000); + + if (pret == 0) { + // Timed out. + PX4_ERR("poll timeout %d, %d", pret, errno); + continue; + } + + if (pret < 0) { + PX4_ERR("poll error %d, %d", pret, errno); + continue; + } + + if (fds[0].revents & POLLIN) { + + int len = ::recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen); + + if (len > 0) { + mavlink_message_t msg; + + for (int i = 0; i < len; i++) { + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &mavlink_status)) { + handle_message(&msg); + } + } + } + } + +#ifdef ENABLE_UART_RC_INPUT + + // got data from PIXHAWK + if (fd_count > 1 && fds[1].revents & POLLIN) { + int len = ::read(serial_fd, serial_buf, sizeof(serial_buf)); + + if (len > 0) { + mavlink_message_t msg; + + mavlink_status_t serial_status = {}; + + for (int i = 0; i < len; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) { + handle_message(&msg); + } + } + } + } + +#endif + } +} + +#ifdef ENABLE_UART_RC_INPUT +int openUart(const char *uart_name, int baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + PX4_ERR("Unsupported baudrate: %d", baud); + return -EINVAL; + } + + /* open uart */ + int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + + if (uart_fd < 0) { + return uart_fd; + } + + + /* Try to set baud rate */ + struct termios uart_config = {}; + + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { + PX4_ERR("tcgetattr failed for %s: %s\n", uart_name, strerror(errno)); + ::close(uart_fd); + return -1; + } + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + PX4_ERR("cfsetispeed or cfsetospeed failed for %s: %s\n", uart_name, strerror(errno)); + ::close(uart_fd); + return -1; + } + + // Make raw + cfmakeraw(&uart_config); + + if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("tcsetattr failed for %s: %s\n", uart_name, strerror(errno)); + ::close(uart_fd); + return -1; + } + + return uart_fd; +} +#endif + +void Simulator::check_failure_injections() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + continue; + } + + bool handled = false; + bool supported = false; + + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + const int instance = static_cast(vehicle_command.param3 + 0.5f); + + if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, GPS off"); + supported = true; + _gps_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); + supported = true; + _gps_blocked = false; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i); + _accel_blocked[i] = true; + _accel_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1); + _accel_blocked[instance - 1] = true; + _accel_stuck[instance - 1] = false; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i); + _accel_blocked[i] = false; + _accel_stuck[i] = true; + } + + } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1); + _accel_blocked[instance - 1] = false; + _accel_stuck[instance - 1] = true; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i); + _accel_blocked[i] = false; + _accel_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1); + _accel_blocked[instance - 1] = false; + _accel_stuck[instance - 1] = false; + } + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i); + _gyro_blocked[i] = true; + _gyro_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1); + _gyro_blocked[instance - 1] = true; + _gyro_stuck[instance - 1] = false; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i); + _gyro_blocked[i] = false; + _gyro_stuck[i] = true; + } + + } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1); + _gyro_blocked[instance - 1] = false; + _gyro_stuck[instance - 1] = true; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i); + _gyro_blocked[i] = false; + _gyro_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1); + _gyro_blocked[instance - 1] = false; + _gyro_stuck[instance - 1] = false; + } + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, mag off"); + supported = true; + _mag_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + PX4_WARN("CMD_INJECT_FAILURE, mag stuck"); + supported = true; + _mag_stuck = true; + _mag_blocked = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, mag ok"); + supported = true; + _mag_blocked = false; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, baro off"); + supported = true; + _baro_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); + supported = true; + _baro_stuck = true; + _baro_blocked = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, baro ok"); + supported = true; + _baro_blocked = false; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); + supported = true; + _airspeed_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); + supported = true; + _airspeed_blocked = false; + } + } + + if (handled) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } + } +} + +int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink) +{ + optical_flow_s flow = {}; + flow.sensor_id = flow_mavlink->sensor_id; + flow.timestamp = hrt_absolute_time(); + flow.time_since_last_sonar_update = 0; + flow.frame_count_since_last_readout = 0; // ? + flow.integration_timespan = flow_mavlink->integration_time_us; + + flow.ground_distance_m = flow_mavlink->distance; + flow.gyro_temperature = flow_mavlink->temperature; + flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro; + flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro; + flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro; + flow.pixel_flow_x_integral = flow_mavlink->integrated_x; + flow.pixel_flow_y_integral = flow_mavlink->integrated_y; + flow.quality = flow_mavlink->quality; + + /* fill in sensor limits */ + float flow_rate_max; + param_get(param_find("SENS_FLOW_MAXR"), &flow_rate_max); + float flow_min_hgt; + param_get(param_find("SENS_FLOW_MINHGT"), &flow_min_hgt); + float flow_max_hgt; + param_get(param_find("SENS_FLOW_MAXHGT"), &flow_max_hgt); + + flow.max_flow_rate = flow_rate_max; + flow.min_ground_distance = flow_min_hgt; + flow.max_ground_distance = flow_max_hgt; + + /* rotate measurements according to parameter */ + int32_t flow_rot_int; + param_get(param_find("SENS_FLOW_ROT"), &flow_rot_int); + const enum Rotation flow_rot = (Rotation)flow_rot_int; + + float zeroval = 0.0f; + rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval); + rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral); + + _flow_pub.publish(flow); + + return PX4_OK; +} + +int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) +{ + uint64_t timestamp = hrt_absolute_time(); + + struct vehicle_odometry_s odom; + + odom.timestamp = timestamp; + odom.timestamp_sample = timestamp; + + const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]); + + if (odom_mavlink->msgid == MAVLINK_MSG_ID_ODOMETRY) { + mavlink_odometry_t odom_msg; + mavlink_msg_odometry_decode(odom_mavlink, &odom_msg); + + /* The position in the local NED frame */ + odom.x = odom_msg.x; + odom.y = odom_msg.y; + odom.z = odom_msg.z; + + /* The quaternion of the ODOMETRY msg represents a rotation from + * NED earth/local frame to XYZ body frame */ + matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]); + q.copyTo(odom.q); + + if (odom_msg.frame_id == MAV_FRAME_LOCAL_NED) { + odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + } else { + odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; + } + + static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])), + "Odometry Pose Covariance matrix URT array size mismatch"); + + /* The pose covariance URT */ + for (size_t i = 0; i < POS_URT_SIZE; i++) { + odom.pose_covariance[i] = odom_msg.pose_covariance[i]; + } + + /* The velocity in the body-fixed frame */ + odom.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD; + odom.vx = odom_msg.vx; + odom.vy = odom_msg.vy; + odom.vz = odom_msg.vz; + + /* The angular velocity in the body-fixed frame */ + odom.rollspeed = odom_msg.rollspeed; + odom.pitchspeed = odom_msg.pitchspeed; + odom.yawspeed = odom_msg.yawspeed; + + // velocity_covariance + static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]); + static_assert(VEL_URT_SIZE == (sizeof(odom_msg.velocity_covariance) / sizeof(odom_msg.velocity_covariance[0])), + "Odometry Velocity Covariance matrix URT array size mismatch"); + + /* The velocity covariance URT */ + for (size_t i = 0; i < VEL_URT_SIZE; i++) { + odom.velocity_covariance[i] = odom_msg.velocity_covariance[i]; + } + + /* Publish the odometry based on the source */ + if (odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VISION || odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VIO) { + _visual_odometry_pub.publish(odom); + + } else if (odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_MOCAP) { + _mocap_odometry_pub.publish(odom); + + } else { + PX4_ERR("Estimator source %u not supported. Unable to publish pose and velocity", odom_msg.estimator_type); + } + + } else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) { + mavlink_vision_position_estimate_t ev; + mavlink_msg_vision_position_estimate_decode(odom_mavlink, &ev); + /* The position in the local NED frame */ + odom.x = ev.x; + odom.y = ev.y; + odom.z = ev.z; + /* The euler angles of the VISUAL_POSITION_ESTIMATE msg represent a + * rotation from NED earth/local frame to XYZ body frame */ + matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw)); + q.copyTo(odom.q); + + odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + static_assert(POS_URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])), + "Vision Position Estimate Pose Covariance matrix URT array size mismatch"); + + /* The pose covariance URT */ + for (size_t i = 0; i < POS_URT_SIZE; i++) { + odom.pose_covariance[i] = ev.covariance[i]; + } + + /* The velocity in the local NED frame - unknown */ + odom.vx = NAN; + odom.vy = NAN; + odom.vz = NAN; + /* The angular velocity in body-fixed frame - unknown */ + odom.rollspeed = NAN; + odom.pitchspeed = NAN; + odom.yawspeed = NAN; + + /* The velocity covariance URT - unknown */ + odom.velocity_covariance[0] = NAN; + + /* Publish the odometry */ + _visual_odometry_pub.publish(odom); + } + + return PX4_OK; +} + +int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink) +{ + distance_sensor_s dist{}; + dist.timestamp = hrt_absolute_time(); + dist.min_distance = dist_mavlink->min_distance / 100.0f; + dist.max_distance = dist_mavlink->max_distance / 100.0f; + dist.current_distance = dist_mavlink->current_distance / 100.0f; + dist.type = dist_mavlink->type; + dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2 + + device::Device::DeviceId device_id {}; + device_id.devid_s.bus_type = device::Device::DeviceBusType_SIMULATION; + device_id.devid_s.address = dist_mavlink->id; + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; + + dist.device_id = device_id.devid; + + // MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown + // quality value. Also it comes normalised between 1 and 100 while the uORB + // signal quality is normalised between 0 and 100. + dist.signal_quality = dist_mavlink->signal_quality == 0 ? -1 : 100 * (dist_mavlink->signal_quality - 1) / 99; + + switch (dist_mavlink->orientation) { + case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270: + dist.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + break; + + case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_90: + dist.orientation = distance_sensor_s::ROTATION_UPWARD_FACING; + break; + + case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_180: + dist.orientation = distance_sensor_s::ROTATION_BACKWARD_FACING; + break; + + case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_NONE: + dist.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + break; + + case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_270: + dist.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + break; + + case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_90: + dist.orientation = distance_sensor_s::ROTATION_RIGHT_FACING; + break; + + default: + dist.orientation = distance_sensor_s::ROTATION_CUSTOM; + } + + dist.h_fov = dist_mavlink->horizontal_fov; + dist.v_fov = dist_mavlink->vertical_fov; + dist.q[0] = dist_mavlink->quaternion[0]; + dist.q[1] = dist_mavlink->quaternion[1]; + dist.q[2] = dist_mavlink->quaternion[2]; + dist.q[3] = dist_mavlink->quaternion[3]; + + // New publishers will be created based on the sensor ID's being different or not + for (size_t i = 0; i < sizeof(_dist_sensor_ids) / sizeof(_dist_sensor_ids[0]); i++) { + if (_dist_pubs[i] && _dist_sensor_ids[i] == dist.device_id) { + _dist_pubs[i]->publish(dist); + break; + + } + + if (_dist_pubs[i] == nullptr) { + _dist_pubs[i] = new uORB::PublicationMulti {ORB_ID(distance_sensor)}; + _dist_sensor_ids[i] = dist.device_id; + _dist_pubs[i]->publish(dist); + break; + } + } + + return PX4_OK; +} diff --git a/src/prometheus_px4/src/modules/simulator/simulator_params.c b/src/prometheus_px4/src/modules/simulator/simulator_params.c new file mode 100644 index 0000000..4a6c211 --- /dev/null +++ b/src/prometheus_px4/src/modules/simulator/simulator_params.c @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 simulator_params.c + * + * Parameters of software in the loop + * + * @author Mohamed Abdelkader + */ diff --git a/src/prometheus_px4/src/modules/temperature_compensation/CMakeLists.txt b/src/prometheus_px4/src/modules/temperature_compensation/CMakeLists.txt new file mode 100644 index 0000000..50e6a18 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015-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__temperature_compensation + MAIN temperature_compensation + SRCS + TemperatureCompensationModule.cpp + TemperatureCompensation.cpp + temperature_calibration/accel.cpp + temperature_calibration/baro.cpp + temperature_calibration/gyro.cpp + temperature_calibration/task.cpp + DEPENDS + mathlib + ) diff --git a/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensation.cpp b/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensation.cpp new file mode 100644 index 0000000..c7508f4 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensation.cpp @@ -0,0 +1,473 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2020, 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 temperature_compensation.cpp + * + * Sensors temperature compensation methods + * + * @author Paul Riseborough + */ + +#include "TemperatureCompensation.h" +#include +#include +#include + +namespace temperature_compensation +{ + +int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶meter_handles) +{ + char nbuf[16] {}; + int ret = PX4_ERROR; + + /* rate gyro calibration parameters */ + parameter_handles.gyro_tc_enable = param_find("TC_G_ENABLE"); + int32_t gyro_tc_enabled = 0; + ret = param_get(parameter_handles.gyro_tc_enable, &gyro_tc_enabled); + + if (ret == PX4_OK && gyro_tc_enabled) { + for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) { + sprintf(nbuf, "TC_G%d_ID", j); + parameter_handles.gyro_cal_handles[j].ID = param_find(nbuf); + + for (unsigned i = 0; i < 3; i++) { + sprintf(nbuf, "TC_G%d_X3_%d", j, i); + parameter_handles.gyro_cal_handles[j].x3[i] = param_find(nbuf); + sprintf(nbuf, "TC_G%d_X2_%d", j, i); + parameter_handles.gyro_cal_handles[j].x2[i] = param_find(nbuf); + sprintf(nbuf, "TC_G%d_X1_%d", j, i); + parameter_handles.gyro_cal_handles[j].x1[i] = param_find(nbuf); + sprintf(nbuf, "TC_G%d_X0_%d", j, i); + parameter_handles.gyro_cal_handles[j].x0[i] = param_find(nbuf); + } + + sprintf(nbuf, "TC_G%d_TREF", j); + parameter_handles.gyro_cal_handles[j].ref_temp = param_find(nbuf); + sprintf(nbuf, "TC_G%d_TMIN", j); + parameter_handles.gyro_cal_handles[j].min_temp = param_find(nbuf); + sprintf(nbuf, "TC_G%d_TMAX", j); + parameter_handles.gyro_cal_handles[j].max_temp = param_find(nbuf); + } + } + + /* accelerometer calibration parameters */ + parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE"); + int32_t accel_tc_enabled = 0; + ret = param_get(parameter_handles.accel_tc_enable, &accel_tc_enabled); + + if (ret == PX4_OK && accel_tc_enabled) { + for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { + sprintf(nbuf, "TC_A%d_ID", j); + parameter_handles.accel_cal_handles[j].ID = param_find(nbuf); + + for (unsigned i = 0; i < 3; i++) { + sprintf(nbuf, "TC_A%d_X3_%d", j, i); + parameter_handles.accel_cal_handles[j].x3[i] = param_find(nbuf); + sprintf(nbuf, "TC_A%d_X2_%d", j, i); + parameter_handles.accel_cal_handles[j].x2[i] = param_find(nbuf); + sprintf(nbuf, "TC_A%d_X1_%d", j, i); + parameter_handles.accel_cal_handles[j].x1[i] = param_find(nbuf); + sprintf(nbuf, "TC_A%d_X0_%d", j, i); + parameter_handles.accel_cal_handles[j].x0[i] = param_find(nbuf); + } + + sprintf(nbuf, "TC_A%d_TREF", j); + parameter_handles.accel_cal_handles[j].ref_temp = param_find(nbuf); + sprintf(nbuf, "TC_A%d_TMIN", j); + parameter_handles.accel_cal_handles[j].min_temp = param_find(nbuf); + sprintf(nbuf, "TC_A%d_TMAX", j); + parameter_handles.accel_cal_handles[j].max_temp = param_find(nbuf); + } + } + + /* barometer calibration parameters */ + parameter_handles.baro_tc_enable = param_find("TC_B_ENABLE"); + int32_t baro_tc_enabled = 0; + ret = param_get(parameter_handles.baro_tc_enable, &baro_tc_enabled); + + if (ret == PX4_OK && baro_tc_enabled) { + for (unsigned j = 0; j < BARO_COUNT_MAX; j++) { + sprintf(nbuf, "TC_B%d_ID", j); + parameter_handles.baro_cal_handles[j].ID = param_find(nbuf); + sprintf(nbuf, "TC_B%d_X5", j); + parameter_handles.baro_cal_handles[j].x5 = param_find(nbuf); + sprintf(nbuf, "TC_B%d_X4", j); + parameter_handles.baro_cal_handles[j].x4 = param_find(nbuf); + sprintf(nbuf, "TC_B%d_X3", j); + parameter_handles.baro_cal_handles[j].x3 = param_find(nbuf); + sprintf(nbuf, "TC_B%d_X2", j); + parameter_handles.baro_cal_handles[j].x2 = param_find(nbuf); + sprintf(nbuf, "TC_B%d_X1", j); + parameter_handles.baro_cal_handles[j].x1 = param_find(nbuf); + sprintf(nbuf, "TC_B%d_X0", j); + parameter_handles.baro_cal_handles[j].x0 = param_find(nbuf); + sprintf(nbuf, "TC_B%d_TREF", j); + parameter_handles.baro_cal_handles[j].ref_temp = param_find(nbuf); + sprintf(nbuf, "TC_B%d_TMIN", j); + parameter_handles.baro_cal_handles[j].min_temp = param_find(nbuf); + sprintf(nbuf, "TC_B%d_TMAX", j); + parameter_handles.baro_cal_handles[j].max_temp = param_find(nbuf); + } + } + + return PX4_OK; +} + +int TemperatureCompensation::parameters_update() +{ + ParameterHandles parameter_handles; + int ret = initialize_parameter_handles(parameter_handles); + + if (ret != 0) { + return ret; + } + + /* rate gyro calibration parameters */ + param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable); + + if (_parameters.gyro_tc_enable == 1) { + for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) { + if (param_get(parameter_handles.gyro_cal_handles[j].ID, &(_parameters.gyro_cal_data[j].ID)) == PX4_OK) { + param_get(parameter_handles.gyro_cal_handles[j].ref_temp, &(_parameters.gyro_cal_data[j].ref_temp)); + param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(_parameters.gyro_cal_data[j].min_temp)); + param_get(parameter_handles.gyro_cal_handles[j].max_temp, &(_parameters.gyro_cal_data[j].max_temp)); + + for (unsigned int i = 0; i < 3; i++) { + param_get(parameter_handles.gyro_cal_handles[j].x3[i], &(_parameters.gyro_cal_data[j].x3[i])); + param_get(parameter_handles.gyro_cal_handles[j].x2[i], &(_parameters.gyro_cal_data[j].x2[i])); + param_get(parameter_handles.gyro_cal_handles[j].x1[i], &(_parameters.gyro_cal_data[j].x1[i])); + param_get(parameter_handles.gyro_cal_handles[j].x0[i], &(_parameters.gyro_cal_data[j].x0[i])); + } + + } else { + // Set all cal values to zero + memset(&_parameters.gyro_cal_data[j], 0, sizeof(_parameters.gyro_cal_data[j])); + + PX4_WARN("FAIL GYRO %d CAL PARAM LOAD - USING DEFAULTS", j); + ret = PX4_ERROR; + } + } + } + + /* accelerometer calibration parameters */ + param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable); + + if (_parameters.accel_tc_enable == 1) { + for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { + if (param_get(parameter_handles.accel_cal_handles[j].ID, &(_parameters.accel_cal_data[j].ID)) == PX4_OK) { + param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(_parameters.accel_cal_data[j].ref_temp)); + param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp)); + param_get(parameter_handles.accel_cal_handles[j].max_temp, &(_parameters.accel_cal_data[j].max_temp)); + + for (unsigned int i = 0; i < 3; i++) { + param_get(parameter_handles.accel_cal_handles[j].x3[i], &(_parameters.accel_cal_data[j].x3[i])); + param_get(parameter_handles.accel_cal_handles[j].x2[i], &(_parameters.accel_cal_data[j].x2[i])); + param_get(parameter_handles.accel_cal_handles[j].x1[i], &(_parameters.accel_cal_data[j].x1[i])); + param_get(parameter_handles.accel_cal_handles[j].x0[i], &(_parameters.accel_cal_data[j].x0[i])); + } + + } else { + // Set all cal values to zero + memset(&_parameters.accel_cal_data[j], 0, sizeof(_parameters.accel_cal_data[j])); + + PX4_WARN("FAIL ACCEL %d CAL PARAM LOAD - USING DEFAULTS", j); + ret = PX4_ERROR; + } + } + } + + /* barometer calibration parameters */ + param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable); + + if (_parameters.baro_tc_enable == 1) { + for (unsigned j = 0; j < BARO_COUNT_MAX; j++) { + if (param_get(parameter_handles.baro_cal_handles[j].ID, &(_parameters.baro_cal_data[j].ID)) == PX4_OK) { + param_get(parameter_handles.baro_cal_handles[j].ref_temp, &(_parameters.baro_cal_data[j].ref_temp)); + param_get(parameter_handles.baro_cal_handles[j].min_temp, &(_parameters.baro_cal_data[j].min_temp)); + param_get(parameter_handles.baro_cal_handles[j].max_temp, &(_parameters.baro_cal_data[j].max_temp)); + param_get(parameter_handles.baro_cal_handles[j].x5, &(_parameters.baro_cal_data[j].x5)); + param_get(parameter_handles.baro_cal_handles[j].x4, &(_parameters.baro_cal_data[j].x4)); + param_get(parameter_handles.baro_cal_handles[j].x3, &(_parameters.baro_cal_data[j].x3)); + param_get(parameter_handles.baro_cal_handles[j].x2, &(_parameters.baro_cal_data[j].x2)); + param_get(parameter_handles.baro_cal_handles[j].x1, &(_parameters.baro_cal_data[j].x1)); + param_get(parameter_handles.baro_cal_handles[j].x0, &(_parameters.baro_cal_data[j].x0)); + + } else { + // Set all cal values to zero + memset(&_parameters.baro_cal_data[j], 0, sizeof(_parameters.baro_cal_data[j])); + + PX4_WARN("FAIL BARO %d CAL PARAM LOAD - USING DEFAULTS", j); + ret = PX4_ERROR; + } + } + } + + /* the offsets might have changed, so make sure to report that change later when applying the + * next corrections + */ + _gyro_data.reset_temperature(); + _accel_data.reset_temperature(); + _baro_data.reset_temperature(); + + return ret; +} + +bool TemperatureCompensation::calc_thermal_offsets_1D(SensorCalData1D &coef, float measured_temp, float &offset) +{ + bool ret = true; + + // clip the measured temperature to remain within the calibration range + float delta_temp; + + if (measured_temp > coef.max_temp) { + delta_temp = coef.max_temp - coef.ref_temp; + ret = false; + + } else if (measured_temp < coef.min_temp) { + delta_temp = coef.min_temp - coef.ref_temp; + ret = false; + + } else { + delta_temp = measured_temp - coef.ref_temp; + + } + + // calulate the offset + float temp_var = delta_temp; + offset = coef.x0 + coef.x1 * temp_var; + temp_var *= delta_temp; + offset += coef.x2 * temp_var; + temp_var *= delta_temp; + offset += coef.x3 * temp_var; + temp_var *= delta_temp; + offset += coef.x4 * temp_var; + temp_var *= delta_temp; + offset += coef.x5 * temp_var; + + return ret; + +} + +bool TemperatureCompensation::calc_thermal_offsets_3D(const SensorCalData3D &coef, float measured_temp, float offset[]) +{ + bool ret = true; + + // clip the measured temperature to remain within the calibration range + float delta_temp; + + if (measured_temp > coef.max_temp) { + delta_temp = coef.max_temp - coef.ref_temp; + ret = false; + + } else if (measured_temp < coef.min_temp) { + delta_temp = coef.min_temp - coef.ref_temp; + ret = false; + + } else { + delta_temp = measured_temp - coef.ref_temp; + + } + + // calulate the offsets + float delta_temp_2 = delta_temp * delta_temp; + float delta_temp_3 = delta_temp_2 * delta_temp; + + for (uint8_t i = 0; i < 3; i++) { + offset[i] = coef.x0[i] + coef.x1[i] * delta_temp + coef.x2[i] * delta_temp_2 + coef.x3[i] * delta_temp_3; + } + + return ret; +} + +int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance) +{ + if (_parameters.gyro_tc_enable != 1) { + return 0; + } + + return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data, GYRO_COUNT_MAX); +} + +int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance) +{ + if (_parameters.accel_tc_enable != 1) { + return 0; + } + + return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data, ACCEL_COUNT_MAX); +} + +int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance) +{ + if (_parameters.baro_tc_enable != 1) { + return 0; + } + + return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data, BARO_COUNT_MAX); +} + +template +int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data, + const T *sensor_cal_data, uint8_t sensor_count_max) +{ + for (int i = 0; i < sensor_count_max; ++i) { + if (device_id == (uint32_t)sensor_cal_data[i].ID) { + sensor_data.device_mapping[topic_instance] = i; + return i; + } + } + + return -1; +} + +int TemperatureCompensation::update_offsets_gyro(int topic_instance, float temperature, float *offsets) +{ + // Check if temperature compensation is enabled + if (_parameters.gyro_tc_enable != 1) { + return 0; + } + + // Map device ID to uORB topic instance + uint8_t mapping = _gyro_data.device_mapping[topic_instance]; + + if (mapping == 255) { + return -1; + } + + // Calculate and update the offsets + calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets); + + // Check if temperature delta is large enough to warrant a new publication + if (fabsf(temperature - _gyro_data.last_temperature[topic_instance]) > 1.0f) { + _gyro_data.last_temperature[topic_instance] = temperature; + return 2; + } + + return 1; +} + +int TemperatureCompensation::update_offsets_accel(int topic_instance, float temperature, float *offsets) +{ + // Check if temperature compensation is enabled + if (_parameters.accel_tc_enable != 1) { + return 0; + } + + // Map device ID to uORB topic instance + uint8_t mapping = _accel_data.device_mapping[topic_instance]; + + if (mapping == 255) { + return -1; + } + + // Calculate and update the offsets + calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets); + + // Check if temperature delta is large enough to warrant a new publication + if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) { + _accel_data.last_temperature[topic_instance] = temperature; + return 2; + } + + return 1; +} + +int TemperatureCompensation::update_offsets_baro(int topic_instance, float temperature, float *offsets) +{ + // Check if temperature compensation is enabled + if (_parameters.baro_tc_enable != 1) { + return 0; + } + + // Map device ID to uORB topic instance + uint8_t mapping = _baro_data.device_mapping[topic_instance]; + + if (mapping == 255) { + return -1; + } + + // Calculate and update the offsets + calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets); + + // Check if temperature delta is large enough to warrant a new publication + if (fabsf(temperature - _baro_data.last_temperature[topic_instance]) > 1.0f) { + _baro_data.last_temperature[topic_instance] = temperature; + return 2; + } + + return 1; +} + +void TemperatureCompensation::print_status() +{ + PX4_INFO("Temperature Compensation:"); + PX4_INFO(" gyro: enabled: %" PRId32, _parameters.gyro_tc_enable); + + if (_parameters.gyro_tc_enable == 1) { + for (int i = 0; i < GYRO_COUNT_MAX; ++i) { + uint8_t mapping = _gyro_data.device_mapping[i]; + + if (_gyro_data.device_mapping[i] != 255) { + PX4_INFO(" using device ID %" PRId32 " for topic instance %i", _parameters.gyro_cal_data[mapping].ID, i); + } + } + } + + PX4_INFO(" accel: enabled: %" PRId32, _parameters.accel_tc_enable); + + if (_parameters.accel_tc_enable == 1) { + for (int i = 0; i < ACCEL_COUNT_MAX; ++i) { + uint8_t mapping = _accel_data.device_mapping[i]; + + if (_accel_data.device_mapping[i] != 255) { + PX4_INFO(" using device ID %" PRId32 " for topic instance %i", _parameters.accel_cal_data[mapping].ID, i); + } + } + } + + PX4_INFO(" baro: enabled: %" PRId32, _parameters.baro_tc_enable); + + if (_parameters.baro_tc_enable == 1) { + for (int i = 0; i < BARO_COUNT_MAX; ++i) { + uint8_t mapping = _baro_data.device_mapping[i]; + + if (_baro_data.device_mapping[i] != 255) { + PX4_INFO(" using device ID %" PRId32 " for topic instance %i", _parameters.baro_cal_data[mapping].ID, i); + } + } + } +} + +} // namespace temperature_compensation diff --git a/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensation.h b/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensation.h new file mode 100644 index 0000000..051bc73 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensation.h @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 temperature_compensation.h + * + * Sensor correction methods + * + * @author Paul Riseborough + * @author Beat Küng + */ + +#pragma once + +#include +#include +#include + +namespace temperature_compensation +{ + +static constexpr uint8_t GYRO_COUNT_MAX = 4; +static constexpr uint8_t ACCEL_COUNT_MAX = 4; +static constexpr uint8_t BARO_COUNT_MAX = 4; + +static_assert(GYRO_COUNT_MAX == 4, "GYRO_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)"); +static_assert(ACCEL_COUNT_MAX == 4, + "ACCEL_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)"); +static_assert(BARO_COUNT_MAX == 4, "BARO_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)"); + +static constexpr uint8_t SENSOR_COUNT_MAX = 4; + +/** + ** class TemperatureCompensation + * Applies temperature compensation to sensor data. Loads the parameters from PX4 param storage. + */ +class TemperatureCompensation +{ +public: + + /** (re)load the parameters. Make sure to call this on startup as well */ + int parameters_update(); + + /** supply information which device_id matches a specific uORB topic_instance + * (needed if a system has multiple sensors of the same type) + * @return index for compensation parameter entry containing matching device ID on success, <0 otherwise */ + int set_sensor_id_gyro(uint32_t device_id, int topic_instance); + int set_sensor_id_accel(uint32_t device_id, int topic_instance); + int set_sensor_id_baro(uint32_t device_id, int topic_instance); + + /** + * Apply Thermal corrections to gyro (& other) sensor data. + * @param topic_instance uORB topic instance + * @param sensor_data input sensor data, output sensor data with applied corrections + * @param temperature measured current temperature + * @param offsets returns offsets that were applied (length = 3, except for baro), depending on return value + * @return -1: error: correction enabled, but no sensor mapping set (@see set_sendor_id_gyro) + * 0: no changes (correction not enabled), + * 1: corrections applied but no changes to offsets, + * 2: corrections applied and offsets updated + */ + int update_offsets_gyro(int topic_instance, float temperature, float *offsets); + int update_offsets_accel(int topic_instance, float temperature, float *offsets); + int update_offsets_baro(int topic_instance, float temperature, float *offsets); + + /** output current configuration status to console */ + void print_status(); +private: + + /* Struct containing parameters used by the single axis 5th order temperature compensation algorithm + + Input: + + measured_temp : temperature measured at the sensor (deg C) + raw_value : reading from the sensor before compensation + corrected_value : reading from the sensor after compensation for errors + + Compute: + + delta_temp = measured_temp - ref_temp + offset = x5 * delta_temp^5 + x4 * delta_temp^4 + x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0 + corrected_value = raw_value - offset + + */ + struct SensorCalData1D { + int32_t ID; + float x5; + float x4; + float x3; + float x2; + float x1; + float x0; + float ref_temp; + float min_temp; + float max_temp; + }; + + struct SensorCalHandles1D { + param_t ID; + param_t x5; + param_t x4; + param_t x3; + param_t x2; + param_t x1; + param_t x0; + param_t ref_temp; + param_t min_temp; + param_t max_temp; + }; + + + /* Struct containing parameters used by the 3-axis 3rd order temperature compensation algorithm + + Input: + + measured_temp : temperature measured at the sensor (deg C) + raw_value[3] : XYZ readings from the sensor before compensation + corrected_value[3] : XYZ readings from the sensor after compensation for errors + + Compute for each measurement index: + + delta_temp = measured_temp - ref_temp + offset = x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0 + corrected_value = raw_value - offset + + */ + struct SensorCalData3D { + int32_t ID; /**< sensor device ID*/ + float x3[3]; /**< x^3 term of polynomial */ + float x2[3]; /**< x^2 term of polynomial */ + float x1[3]; /**< x^1 term of polynomial */ + float x0[3]; /**< x^0 / offset term of polynomial */ + float ref_temp; /**< reference temperature used by the curve-fit */ + float min_temp; /**< minimum temperature with valid compensation data */ + float max_temp; /**< maximum temperature with valid compensation data */ + }; + + struct SensorCalHandles3D { + param_t ID; + param_t x3[3]; + param_t x2[3]; + param_t x1[3]; + param_t x0[3]; + param_t ref_temp; + param_t min_temp; + param_t max_temp; + }; + + // create a struct containing all thermal calibration parameters + struct Parameters { + int32_t gyro_tc_enable{0}; + SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX] {}; + + int32_t accel_tc_enable{0}; + SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX] {}; + + int32_t baro_tc_enable{0}; + SensorCalData1D baro_cal_data[BARO_COUNT_MAX] {}; + }; + + // create a struct containing the handles required to access all calibration parameters + struct ParameterHandles { + param_t gyro_tc_enable{PARAM_INVALID}; + SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX] {}; + + param_t accel_tc_enable{PARAM_INVALID}; + SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX] {}; + + param_t baro_tc_enable{PARAM_INVALID}; + SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX] {}; + }; + + + /** + * initialize ParameterHandles struct + * @return 0 on succes, <0 on error + */ + static int initialize_parameter_handles(ParameterHandles ¶meter_handles); + + + /** + + Calculate the offset required to compensate the sensor for temperature effects using a 5th order method + If the measured temperature is outside the calibration range, clip the temperature to remain within the range and return false. + If the measured temperature is within the calibration range, return true. + + Arguments: + + coef : reference to struct containing calibration coefficients + measured_temp : temperature measured at the sensor (deg C) + offset : reference to sensor offset + + Returns: + + Boolean true if the measured temperature is inside the valid range for the compensation + + */ + bool calc_thermal_offsets_1D(SensorCalData1D &coef, float measured_temp, float &offset); + + /** + + Calculate the offsets required to compensate the sensor for temperature effects + If the measured temperature is outside the calibration range, clip the temperature to remain within the range and return false. + If the measured temperature is within the calibration range, return true. + + Arguments: + + coef : reference to struct containing calibration coefficients + measured_temp : temperature measured at the sensor (deg C) + offset : reference to sensor offset - array of 3 + + Returns: + + Boolean true if the measured temperature is inside the valid range for the compensation + + */ + bool calc_thermal_offsets_3D(const SensorCalData3D &coef, float measured_temp, float offset[]); + + + Parameters _parameters; + + + struct PerSensorData { + + PerSensorData() + { + for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { + device_mapping[i] = 255; + last_temperature[i] = -100.0f; + } + } + + void reset_temperature() + { + for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { + last_temperature[i] = -100.0f; + } + } + + uint8_t device_mapping[SENSOR_COUNT_MAX] {}; /// map a topic instance to the parameters index + float last_temperature[SENSOR_COUNT_MAX] {}; + }; + + PerSensorData _gyro_data; + PerSensorData _accel_data; + PerSensorData _baro_data; + + template + static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data, + const T *sensor_cal_data, uint8_t sensor_count_max); +}; + +} diff --git a/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensationModule.cpp new file mode 100644 index 0000000..0d826f4 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -0,0 +1,399 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2020, 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 "TemperatureCompensationModule.h" + +#include "temperature_calibration/temperature_calibration.h" + +#include +#include + +#include + +using namespace temperature_compensation; + +TemperatureCompensationModule::TemperatureCompensationModule() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), + _loop_perf(perf_alloc(PC_ELAPSED, "temperature_compensation")) +{ +} + +TemperatureCompensationModule::~TemperatureCompensationModule() +{ + perf_free(_loop_perf); +} + +void TemperatureCompensationModule::parameters_update() +{ + _temperature_compensation.parameters_update(); + + // Gyro + for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) { + sensor_gyro_s report; + + if (_gyro_subs[uorb_index].copy(&report)) { + int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, uorb_index); + + if (temp < 0) { + PX4_INFO("No temperature calibration available for gyro %" PRIu8 " (device id %" PRIu32 ")", uorb_index, + report.device_id); + _corrections.gyro_device_ids[uorb_index] = 0; + + } else { + _corrections.gyro_device_ids[uorb_index] = report.device_id; + } + } + } + + // Accel + for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) { + sensor_accel_s report; + + if (_accel_subs[uorb_index].copy(&report)) { + int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index); + + if (temp < 0) { + PX4_INFO("No temperature calibration available for accel %" PRIu8 " (device id %" PRIu32 ")", uorb_index, + report.device_id); + _corrections.accel_device_ids[uorb_index] = 0; + + } else { + _corrections.accel_device_ids[uorb_index] = report.device_id; + } + } + } + + // Baro + for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) { + sensor_baro_s report; + + if (_baro_subs[uorb_index].copy(&report)) { + int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, uorb_index); + + if (temp < 0) { + PX4_INFO("No temperature calibration available for baro %" PRIu8 " (device id %" PRIu32 ")", uorb_index, + report.device_id); + _corrections.baro_device_ids[uorb_index] = 0; + + } else { + _corrections.baro_device_ids[uorb_index] = temp; + } + } + } +} + +void TemperatureCompensationModule::accelPoll() +{ + float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2, _corrections.accel_offset_3 }; + + // For each accel instance + for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) { + sensor_accel_s report; + + // Grab temperature from report + if (_accel_subs[uorb_index].update(&report)) { + if (PX4_ISFINITE(report.temperature)) { + // Update the offsets and mark for publication if they've changed + if (_temperature_compensation.update_offsets_accel(uorb_index, report.temperature, offsets[uorb_index]) == 2) { + + _corrections.accel_device_ids[uorb_index] = report.device_id; + _corrections_changed = true; + } + } + } + } +} + +void TemperatureCompensationModule::gyroPoll() +{ + float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2, _corrections.gyro_offset_3 }; + + // For each gyro instance + for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) { + sensor_gyro_s report; + + // Grab temperature from report + if (_gyro_subs[uorb_index].update(&report)) { + if (PX4_ISFINITE(report.temperature)) { + // Update the offsets and mark for publication if they've changed + if (_temperature_compensation.update_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index]) == 2) { + + _corrections.gyro_device_ids[uorb_index] = report.device_id; + _corrections_changed = true; + } + } + } + } +} + +void TemperatureCompensationModule::baroPoll() +{ + float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2, &_corrections.baro_offset_3 }; + + // For each baro instance + for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) { + sensor_baro_s report; + + // Grab temperature from report + if (_baro_subs[uorb_index].update(&report)) { + if (PX4_ISFINITE(report.temperature)) { + // Update the offsets and mark for publication if they've changed + if (_temperature_compensation.update_offsets_baro(uorb_index, report.temperature, offsets[uorb_index]) == 2) { + + _corrections.baro_device_ids[uorb_index] = report.device_id; + _corrections_changed = true; + } + } + } + } +} + +void TemperatureCompensationModule::Run() +{ + perf_begin(_loop_perf); + + // Check if user has requested to run the calibration routine + while (_vehicle_command_sub.updated()) { + vehicle_command_s cmd; + + if (_vehicle_command_sub.copy(&cmd)) { + if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) { + bool got_temperature_calibration_command = false; + bool accel = false; + bool baro = false; + bool gyro = false; + + if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + gyro = true; + got_temperature_calibration_command = true; + } + + if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + accel = true; + got_temperature_calibration_command = true; + } + + if ((int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + baro = true; + got_temperature_calibration_command = true; + } + + if (got_temperature_calibration_command) { + int ret = run_temperature_calibration(accel, baro, gyro); + + // publish ACK + vehicle_command_ack_s command_ack{}; + + if (ret == 0) { + command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED; + } + + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = cmd.command; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + + uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); + } + } + } + } + + // Check if any parameter has changed + if (_parameter_update_sub.updated()) { + // Read from param to clear updated flag + parameter_update_s update; + _parameter_update_sub.copy(&update); + + parameters_update(); + } + + accelPoll(); + gyroPoll(); + baroPoll(); + + // publish sensor corrections if necessary + if (_corrections_changed) { + _corrections.timestamp = hrt_absolute_time(); + + _sensor_correction_pub.publish(_corrections); + + _corrections_changed = false; + } + + perf_end(_loop_perf); +} + +int TemperatureCompensationModule::task_spawn(int argc, char *argv[]) +{ + TemperatureCompensationModule *instance = new TemperatureCompensationModule(); + + 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 TemperatureCompensationModule::init() +{ + ScheduleOnInterval(1_s); + return true; +} + +int TemperatureCompensationModule::custom_command(int argc, char *argv[]) +{ + if (!strcmp(argv[0], "calibrate")) { + + bool accel_calib = false; + bool baro_calib = false; + bool gyro_calib = false; + bool calib_all = true; + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'a': + accel_calib = true; + calib_all = false; + break; + + case 'b': + baro_calib = true; + calib_all = false; + break; + + case 'g': + gyro_calib = true; + calib_all = false; + break; + + default: + print_usage("unrecognized flag"); + + return PX4_ERROR; + } + } + + if (!is_running()) { + PX4_WARN("background task not running"); + + if (task_spawn(0, nullptr) != PX4_OK) { + return PX4_ERROR; + } + } + + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); + vcmd.param1 = (float)((gyro_calib + || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); + vcmd.param2 = NAN; + vcmd.param3 = NAN; + vcmd.param4 = NAN; + vcmd.param5 = ((accel_calib + || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN); + vcmd.param6 = (double)NAN; + vcmd.param7 = (float)((baro_calib + || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); + vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; + + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); + + return PX4_OK; + + } else { + print_usage("unrecognized command"); + + return PX4_ERROR; + } +} + +int TemperatureCompensationModule::print_status() +{ + _temperature_compensation.print_status(); + + return PX4_OK; +} + +int TemperatureCompensationModule::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The temperature compensation module allows all of the gyro(s), accel(s), and baro(s) in the system to be temperature +compensated. The module monitors the data coming from the sensors and updates the associated sensor_correction topic +whenever a change in temperature is detected. The module can also be configured to perform the coeffecient calculation +routine at next boot, which allows the thermal calibration coeffecients to be calculated while the vehicle undergoes +a temperature cycle. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("temperature_compensation", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module, which monitors the sensors and updates the sensor_correction topic"); + PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run temperature calibration process"); + PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true); + PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true); + PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int temperature_compensation_main(int argc, char *argv[]) +{ + return TemperatureCompensationModule::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensationModule.h b/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensationModule.h new file mode 100644 index 0000000..24e9ea5 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/TemperatureCompensationModule.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "TemperatureCompensation.h" + +using namespace time_literals; + +namespace temperature_compensation +{ + +class TemperatureCompensationModule : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + TemperatureCompensationModule(); + ~TemperatureCompensationModule() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static TemperatureCompensationModule *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::print_status() */ + int print_status() override; + + /** + * Initializes scheduling on work queue. + */ + bool init(); + +private: + + void Run() override; + + void accelPoll(); + void gyroPoll(); + void baroPoll(); + + /** + * call this whenever parameters got updated. Make sure to have initialize_sensors() called at least + * once before calling this. + */ + void parameters_update(); + + uORB::Subscription _accel_subs[ACCEL_COUNT_MAX] { + {ORB_ID(sensor_accel), 0}, + {ORB_ID(sensor_accel), 1}, + {ORB_ID(sensor_accel), 2}, + {ORB_ID(sensor_accel), 3}, + }; + + uORB::Subscription _gyro_subs[GYRO_COUNT_MAX] { + {ORB_ID(sensor_gyro), 0}, + {ORB_ID(sensor_gyro), 1}, + {ORB_ID(sensor_gyro), 2}, + {ORB_ID(sensor_gyro), 3}, + }; + + uORB::Subscription _baro_subs[BARO_COUNT_MAX] { + {ORB_ID(sensor_baro), 0}, + {ORB_ID(sensor_baro), 1}, + {ORB_ID(sensor_baro), 2}, + {ORB_ID(sensor_baro), 3}, + }; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + orb_advert_t _mavlink_log_pub{nullptr}; + + /* sensor thermal compensation */ + TemperatureCompensation _temperature_compensation; + + sensor_correction_s _corrections{}; /**< struct containing the sensor corrections to be published to the uORB*/ + uORB::Publication _sensor_correction_pub{ORB_ID(sensor_correction)}; + + bool _corrections_changed{true}; +}; + +} // namespace temperature_compensation diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel.c new file mode 100644 index 0000000..f42e7ac --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/** + * Thermal compensation for accelerometer sensors. + * + * @group Thermal Compensation + * @reboot_required true + * @boolean + */ +PARAM_DEFINE_INT32(TC_A_ENABLE, 0); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_0.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_0.c new file mode 100644 index 0000000..1b4f034 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_0.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Accelerometer 0 */ + +/** + * ID of Accelerometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_A0_ID, 0); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X3_0, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X3_1, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X3_2, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X2_0, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X2_1, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X2_2, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X1_0, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X1_1, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X1_2, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X0_0, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X0_1, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_X0_2, 0.0f); + +/** + * Accelerometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_TREF, 25.0f); + +/** + * Accelerometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_TMIN, 0.0f); + +/** + * Accelerometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A0_TMAX, 100.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_1.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_1.c new file mode 100644 index 0000000..ed3d0b0 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_1.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Accelerometer 1 */ + +/** + * ID of Accelerometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_A1_ID, 0); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X3_0, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X3_1, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X3_2, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X2_0, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X2_1, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X2_2, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X1_0, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X1_1, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X1_2, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X0_0, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X0_1, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_X0_2, 0.0f); + +/** + * Accelerometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_TREF, 25.0f); + +/** + * Accelerometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_TMIN, 0.0f); + +/** + * Accelerometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A1_TMAX, 100.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_2.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_2.c new file mode 100644 index 0000000..0155a2a --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_2.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Accelerometer 2 */ + +/** + * ID of Accelerometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_A2_ID, 0); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X3_0, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X3_1, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X3_2, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X2_0, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X2_1, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X2_2, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X1_0, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X1_1, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X1_2, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X0_0, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X0_1, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_X0_2, 0.0f); + +/** + * Accelerometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_TREF, 25.0f); + +/** + * Accelerometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_TMIN, 0.0f); + +/** + * Accelerometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A2_TMAX, 100.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_3.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_3.c new file mode 100644 index 0000000..f3d35cb --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_accel_3.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Accelerometer 3 */ + +/** + * ID of Accelerometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_A3_ID, 0); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X3_0, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X3_1, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X3_2, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X2_0, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X2_1, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X2_2, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X1_0, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X1_1, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X1_2, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X0_0, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X0_1, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_X0_2, 0.0f); + +/** + * Accelerometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_TREF, 25.0f); + +/** + * Accelerometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_TMIN, 0.0f); + +/** + * Accelerometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_A3_TMAX, 100.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro.c new file mode 100644 index 0000000..1e5a1ad --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/** + * Thermal compensation for barometric pressure sensors. + * + * @group Thermal Compensation + * @reboot_required true + * @boolean + */ +PARAM_DEFINE_INT32(TC_B_ENABLE, 0); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_0.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_0.c new file mode 100644 index 0000000..1fef482 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_0.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/* Barometer 0 */ + +/** + * ID of Barometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_B0_ID, 0); + +/** + * Barometer offset temperature ^5 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_X5, 0.0f); + +/** + * Barometer offset temperature ^4 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_X4, 0.0f); + +/** + * Barometer offset temperature ^3 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_X3, 0.0f); + +/** + * Barometer offset temperature ^2 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_X2, 0.0f); + +/** + * Barometer offset temperature ^1 polynomial coefficients. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_X1, 0.0f); + +/** + * Barometer offset temperature ^0 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_X0, 0.0f); + +/** + * Barometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_TREF, 40.0f); + +/** + * Barometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_TMIN, 5.0f); + +/** + * Barometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B0_TMAX, 75.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_1.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_1.c new file mode 100644 index 0000000..2f0fcda --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_1.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Barometer 1 */ + +/** + * ID of Barometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_B1_ID, 0); + +/** + * Barometer offset temperature ^5 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_X5, 0.0f); + +/** + * Barometer offset temperature ^4 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_X4, 0.0f); + +/** + * Barometer offset temperature ^3 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_X3, 0.0f); + +/** + * Barometer offset temperature ^2 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_X2, 0.0f); + +/** + * Barometer offset temperature ^1 polynomial coefficients. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_X1, 0.0f); + +/** + * Barometer offset temperature ^0 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_X0, 0.0f); + +/** + * Barometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_TREF, 40.0f); + +/** + * Barometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_TMIN, 5.0f); + +/** + * Barometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B1_TMAX, 75.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_2.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_2.c new file mode 100644 index 0000000..b32d9ef --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_2.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Barometer 2 */ + +/** + * ID of Barometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_B2_ID, 0); + +/** + * Barometer offset temperature ^5 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_X5, 0.0f); + +/** + * Barometer offset temperature ^4 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_X4, 0.0f); + +/** + * Barometer offset temperature ^3 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_X3, 0.0f); + +/** + * Barometer offset temperature ^2 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_X2, 0.0f); + +/** + * Barometer offset temperature ^1 polynomial coefficients. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_X1, 0.0f); + +/** + * Barometer offset temperature ^0 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_X0, 0.0f); + +/** + * Barometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_TREF, 40.0f); + +/** + * Barometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_TMIN, 5.0f); + +/** + * Barometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B2_TMAX, 75.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_3.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_3.c new file mode 100644 index 0000000..a514e78 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_baro_3.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Barometer 3 */ + +/** + * ID of Barometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_B3_ID, 0); + +/** + * Barometer offset temperature ^5 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_X5, 0.0f); + +/** + * Barometer offset temperature ^4 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_X4, 0.0f); + +/** + * Barometer offset temperature ^3 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_X3, 0.0f); + +/** + * Barometer offset temperature ^2 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_X2, 0.0f); + +/** + * Barometer offset temperature ^1 polynomial coefficients. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_X1, 0.0f); + +/** + * Barometer offset temperature ^0 polynomial coefficient. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_X0, 0.0f); + +/** + * Barometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_TREF, 40.0f); + +/** + * Barometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_TMIN, 5.0f); + +/** + * Barometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_B3_TMAX, 75.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro.c new file mode 100644 index 0000000..cd321dd --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/** + * Thermal compensation for rate gyro sensors. + * + * @group Thermal Compensation + * @reboot_required true + * @boolean + */ +PARAM_DEFINE_INT32(TC_G_ENABLE, 0); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_0.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_0.c new file mode 100644 index 0000000..7f05b14 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_0.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Gyro 0 */ + +/** + * ID of Gyro that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_G0_ID, 0); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X3_0, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X3_1, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X3_2, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X2_0, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X2_1, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X2_2, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X1_0, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X1_1, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X1_2, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X0_0, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X0_1, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_X0_2, 0.0f); + +/** + * Gyro calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_TREF, 25.0f); + +/** + * Gyro calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_TMIN, 0.0f); + +/** + * Gyro calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G0_TMAX, 100.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_1.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_1.c new file mode 100644 index 0000000..b86dea2 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_1.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Gyro 1 */ + +/** + * ID of Gyro that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_G1_ID, 0); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X3_0, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X3_1, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X3_2, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X2_0, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X2_1, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X2_2, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X1_0, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X1_1, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X1_2, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X0_0, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X0_1, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_X0_2, 0.0f); + +/** + * Gyro calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_TREF, 25.0f); + +/** + * Gyro calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_TMIN, 0.0f); + +/** + * Gyro calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G1_TMAX, 100.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_2.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_2.c new file mode 100644 index 0000000..ed20377 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_2.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Gyro 2 */ + +/** + * ID of Gyro that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_G2_ID, 0); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X3_0, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X3_1, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X3_2, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X2_0, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X2_1, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X2_2, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X1_0, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X1_1, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X1_2, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X0_0, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X0_1, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_X0_2, 0.0f); + +/** + * Gyro calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_TREF, 25.0f); + +/** + * Gyro calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_TMIN, 0.0f); + +/** + * Gyro calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G2_TMAX, 100.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_3.c b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_3.c new file mode 100644 index 0000000..a0ed93a --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temp_comp_params_gyro_3.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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. + * + ****************************************************************************/ + +/* Gyro 3 */ + +/** + * ID of Gyro that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_G3_ID, 0); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X3_0, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X3_1, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X3_2, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X2_0, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X2_1, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X2_2, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X1_0, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X1_1, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X1_2, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X0_0, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X0_1, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_X0_2, 0.0f); + +/** + * Gyro calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_TREF, 25.0f); + +/** + * Gyro calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_TMIN, 0.0f); + +/** + * Gyro calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_G3_TMAX, 100.0f); diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/accel.cpp b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/accel.cpp new file mode 100644 index 0000000..d9d4ef2 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/accel.cpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * Copyright (c) 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 accel.cpp + * Implementation of the Accel Temperature Calibration for onboard sensors. + * + * @author Siddharth Bharat Purohit + * @author Paul Riseborough + * @author Beat Küng + */ + +#include "accel.h" +#include +#include +#include + +TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, + float max_start_temperature) + : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) +{ + // init subscriptions + _num_sensor_instances = orb_group_count(ORB_ID(sensor_accel)); + + if (_num_sensor_instances > SENSOR_COUNT_MAX) { + _num_sensor_instances = SENSOR_COUNT_MAX; + } + + for (unsigned i = 0; i < _num_sensor_instances; i++) { + _sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); + } +} + +TemperatureCalibrationAccel::~TemperatureCalibrationAccel() +{ + for (unsigned i = 0; i < _num_sensor_instances; i++) { + orb_unsubscribe(_sensor_subs[i]); + } +} + +int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub) +{ + bool finished = data.hot_soaked; + + bool updated; + orb_check(sensor_sub, &updated); + + if (!updated) { + return finished ? 0 : 1; + } + + sensor_accel_s accel_data; + orb_copy(ORB_ID(sensor_accel), sensor_sub, &accel_data); + + if (finished) { + // if we're done, return, but we need to return after orb_copy because of poll() + return 0; + } + + if (PX4_ISFINITE(accel_data.temperature)) { + data.has_valid_temperature = true; + + } else { + return 0; + } + + data.device_id = accel_data.device_id; + + data.sensor_sample_filt[0] = accel_data.x; + data.sensor_sample_filt[1] = accel_data.y; + data.sensor_sample_filt[2] = accel_data.z; + data.sensor_sample_filt[3] = accel_data.temperature; + + // wait for min start temp to be reached before starting calibration + if (data.sensor_sample_filt[3] < _min_start_temperature) { + return 1; + } + + if (!data.cold_soaked) { + // allow time for sensors and filters to settle + if (hrt_absolute_time() > 10E6) { + // If intial temperature exceeds maximum declare an error condition and exit + if (data.sensor_sample_filt[3] > _max_start_temperature) { + return -TC_ERROR_INITIAL_TEMP_TOO_HIGH; + + } else { + data.cold_soaked = true; + data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature + data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature + data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise; + return 1; + } + + } else { + return 1; + } + } + + // check if temperature increased + if (data.sensor_sample_filt[3] > data.high_temp) { + data.high_temp = data.sensor_sample_filt[3]; + data.hot_soak_sat = 0; + + } else { + return 1; + } + + //TODO: Detect when temperature has stopped rising for more than TBD seconds + if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) { + data.hot_soaked = true; + } + + if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor + TC_DEBUG("\nAccel: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0], + (double)data.sensor_sample_filt[1], + (double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp, + (double)(data.high_temp - data.low_temp)); + } + + //update linear fit matrices + double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp; + data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]); + data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]); + data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]); + + return 1; +} + +int TemperatureCalibrationAccel::finish() +{ + for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { + finish_sensor_instance(_data[uorb_index], uorb_index); + } + + int32_t enabled = 1; + int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled); + + if (result != PX4_OK) { + PX4_ERR("unable to reset TC_A_ENABLE (%i)", result); + } + + return result; +} + +int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int sensor_index) +{ + if (!data.has_valid_temperature) { + PX4_WARN("Result Accel %d does not have a valid temperature sensor", sensor_index); + + uint32_t param = 0; + set_parameter("TC_A%d_ID", sensor_index, ¶m); + return 0; + } + + if (!data.hot_soaked || data.tempcal_complete) { + return 0; + } + + double res[3][4] = {}; + data.P[0].fit(res[0]); + res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature + PX4_INFO("Result Accel %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1], + (double)res[0][2], + (double)res[0][3]); + data.P[1].fit(res[1]); + res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature + PX4_INFO("Result Accel %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1], + (double)res[1][2], + (double)res[1][3]); + data.P[2].fit(res[2]); + res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature + PX4_INFO("Result Accel %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1], + (double)res[2][2], + (double)res[2][3]); + data.tempcal_complete = true; + + char str[30]; + float param = 0.0f; + int result = PX4_OK; + + set_parameter("TC_A%d_ID", sensor_index, &data.device_id); + + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + for (unsigned coef_index = 0; coef_index <= 3; coef_index++) { + sprintf(str, "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index); + param = (float)res[axis_index][coef_index]; + result = param_set_no_notification(param_find(str), ¶m); + + if (result != PX4_OK) { + PX4_ERR("unable to reset %s", str); + } + } + } + + set_parameter("TC_A%d_TMAX", sensor_index, &data.high_temp); + set_parameter("TC_A%d_TMIN", sensor_index, &data.low_temp); + set_parameter("TC_A%d_TREF", sensor_index, &data.ref_temp); + return 0; +} diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/accel.h b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/accel.h new file mode 100644 index 0000000..f464bb0 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/accel.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include "common.h" +#include "polyfit.hpp" + +class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3> +{ +public: + TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature); + virtual ~TemperatureCalibrationAccel(); + + /** + * @see TemperatureCalibrationBase::finish() + */ + int finish(); + +private: + + virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub); + + inline int finish_sensor_instance(PerSensorData &data, int sensor_index); +}; diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/baro.cpp b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/baro.cpp new file mode 100644 index 0000000..bd535e3 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/baro.cpp @@ -0,0 +1,213 @@ +/**************************************************************************** + * + * Copyright (c) 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 baro.cpp + * Implementation of the Baro Temperature Calibration for onboard sensors. + * + * @author Siddharth Bharat Purohit + * @author Paul Riseborough + * @author Beat Küng + */ + +#include "baro.h" +#include +#include +#include + +TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, + float max_start_temperature) + : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) +{ + // init subscriptions + _num_sensor_instances = orb_group_count(ORB_ID(sensor_baro)); + + if (_num_sensor_instances > SENSOR_COUNT_MAX) { + _num_sensor_instances = SENSOR_COUNT_MAX; + } + + for (unsigned i = 0; i < _num_sensor_instances; i++) { + _sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i); + } +} + +TemperatureCalibrationBaro::~TemperatureCalibrationBaro() +{ + for (unsigned i = 0; i < _num_sensor_instances; i++) { + orb_unsubscribe(_sensor_subs[i]); + } +} + +int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub) +{ + bool finished = data.hot_soaked; + + bool updated; + orb_check(sensor_sub, &updated); + + if (!updated) { + return finished ? 0 : 1; + } + + sensor_baro_s baro_data; + orb_copy(ORB_ID(sensor_baro), sensor_sub, &baro_data); + + if (finished) { + // if we're done, return, but we need to return after orb_copy because of poll() + return 0; + } + + if (PX4_ISFINITE(baro_data.temperature)) { + data.has_valid_temperature = true; + + } else { + return 0; + } + + data.device_id = baro_data.device_id; + + data.sensor_sample_filt[0] = 100.0f * baro_data.pressure; // convert from hPA to Pa + data.sensor_sample_filt[1] = baro_data.temperature; + + + // wait for min start temp to be reached before starting calibration + if (data.sensor_sample_filt[1] < _min_start_temperature) { + return 1; + } + + if (!data.cold_soaked) { + // allow time for sensors and filters to settle + if (hrt_absolute_time() > 10E6) { + // If intial temperature exceeds maximum declare an error condition and exit + if (data.sensor_sample_filt[1] > _max_start_temperature) { + return -TC_ERROR_INITIAL_TEMP_TOO_HIGH; + + } else { + data.cold_soaked = true; + data.low_temp = data.sensor_sample_filt[1]; // Record the low temperature + data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature + data.ref_temp = data.sensor_sample_filt[1] + 0.5f * _min_temperature_rise; + return 1; + } + + } else { + return 1; + } + } + + // check if temperature increased + if (data.sensor_sample_filt[1] > data.high_temp) { + data.high_temp = data.sensor_sample_filt[1]; + data.hot_soak_sat = 0; + + } else { + return 1; + } + + // TODO: Detect when temperature has stopped rising for more than TBD seconds + if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) { + data.hot_soaked = true; + } + + if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor + TC_DEBUG("\nBaro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0], + (double)data.sensor_sample_filt[1], (double)data.low_temp, (double)data.high_temp, + (double)(data.high_temp - data.low_temp)); + } + + // update linear fit matrices + double relative_temperature = (double)data.sensor_sample_filt[1] - (double)data.ref_temp; + data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]); + + return 1; +} + +int TemperatureCalibrationBaro::finish() +{ + for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { + finish_sensor_instance(_data[uorb_index], uorb_index); + } + + int32_t enabled = 1; + int result = param_set_no_notification(param_find("TC_B_ENABLE"), &enabled); + + if (result != PX4_OK) { + PX4_ERR("unable to reset TC_B_ENABLE (%i)", result); + } + + return result; +} + +int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index) +{ + if (!data.has_valid_temperature) { + PX4_WARN("Result baro %d does not have a valid temperature sensor", sensor_index); + + uint32_t param = 0; + set_parameter("TC_B%d_ID", sensor_index, ¶m); + return 0; + } + + if (!data.hot_soaked || data.tempcal_complete) { + return 0; + } + + double res[POLYFIT_ORDER + 1] {}; + data.P[0].fit(res); + res[POLYFIT_ORDER] = + 0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero + PX4_INFO("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0], + (double)res[1], (double)res[2], (double)res[3], (double)res[4], (double)res[5]); + data.tempcal_complete = true; + + char str[30]; + float param = 0.0f; + int result = PX4_OK; + + set_parameter("TC_B%d_ID", sensor_index, &data.device_id); + + for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) { + sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index)); + param = (float)res[coef_index]; + result = param_set_no_notification(param_find(str), ¶m); + + if (result != PX4_OK) { + PX4_ERR("unable to reset %s", str); + } + } + + set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp); + set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp); + set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp); + return 0; +} diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/baro.h b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/baro.h new file mode 100644 index 0000000..fc74e30 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/baro.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include "common.h" +#include "polyfit.hpp" + +#define POLYFIT_ORDER 5 + + +class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFIT_ORDER> +{ +public: + TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, float max_start_temperature); + virtual ~TemperatureCalibrationBaro(); + + /** + * @see TemperatureCalibrationBase::finish() + */ + int finish(); + +private: + + virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); + + inline int finish_sensor_instance(PerSensorData &data, int sensor_index); +}; diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/common.h b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/common.h new file mode 100644 index 0000000..442ed01 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/common.h @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#define TC_PRINT_DEBUG 0 +#if TC_PRINT_DEBUG +#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__); +#else +#define TC_DEBUG(fmt, ...) +#endif + +#include +#include +#include + +#include "polyfit.hpp" + +#define SENSOR_COUNT_MAX 3 + + +#define TC_ERROR_INITIAL_TEMP_TOO_HIGH 110 ///< starting temperature was above the configured allowed temperature +#define TC_ERROR_COMMUNICATION 112 ///< no sensors found + +/** + * Base class for temperature calibration types with abstract methods (for all different sensor types) + */ +class TemperatureCalibrationBase +{ +public: + TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature, float max_start_temperature) + : _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature), + _max_start_temperature(max_start_temperature) {} + + virtual ~TemperatureCalibrationBase() = default; + + /** + * check & update new sensor data. + * @return progress in range [0, 100], 110 when finished, <0 on error, + * -TC_ERROR_INITIAL_TEMP_TOO_HIGH if starting temperature is too hot + * -TC_ERROR_COMMUNICATION if no sensors found + */ + virtual int update() = 0; + + /** + * do final fitting & write the parameters. Call this exactly once after update() returned 110 + * @return 0 on success, <0 otherwise + */ + virtual int finish() = 0; + +protected: + + /** + * set a system parameter (without system notification) and print an error if it fails + * @param format_str for example "CAL_GYRO%u_XOFF" + * @param index which index (will replace %u in format_str) + * @param value + * @return 0 on success + */ + inline int set_parameter(const char *format_str, unsigned index, const void *value); + + float _min_temperature_rise; ///< minimum difference in temperature before the process finishes + float _min_start_temperature; ///< minimum temperature before the process starts + float _max_start_temperature; ///< maximum temperature above which the process does not start and an error is declared +}; + +int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned index, const void *value) +{ + char param_str[30] {}; + (void)sprintf(param_str, format_str, index); + int result = param_set_no_notification(param_find(param_str), value); + + if (result != 0) { + PX4_ERR("unable to reset %s (%i)", param_str, result); + } + + return result; +} + +/** + ** class TemperatureCalibrationCommon + * Common base class for all sensor types, contains shared code & data. + */ +template +class TemperatureCalibrationCommon : public TemperatureCalibrationBase +{ +public: + TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature, float max_start_temperature) + : TemperatureCalibrationBase(min_temperature_rise, min_start_temperature, max_start_temperature) {} + + virtual ~TemperatureCalibrationCommon() = default; + + /** + * @see TemperatureCalibrationBase::update() + */ + int update() + { + int num_not_complete = 0; + + if (_num_sensor_instances == 0) { + return -TC_ERROR_COMMUNICATION; + } + + for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { + int status = update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]); + + if (status < 0) { + return status; + } + + num_not_complete += status; + } + + if (num_not_complete > 0) { + // calculate progress + float min_diff = _min_temperature_rise; + + for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { + if (!_data[uorb_index].has_valid_temperature) { + return 110; + } + + float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp; + + if (cur_diff < min_diff) { + min_diff = cur_diff; + } + } + + return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f)); + } + + return 110; + } + +protected: + + struct PerSensorData { + float sensor_sample_filt[Dim + 1]; ///< last value is the temperature + polyfitter < PolyfitOrder + 1 > P[Dim]; + unsigned hot_soak_sat = 0; /**< counter that increments every time the sensor temperature reduces + from the last reading */ + uint32_t device_id = 0; ///< ID for the sensor being calibrated + bool cold_soaked = false; ///< true when the sensor cold soak starting temperature condition had been + /// verified and the starting temperature set + bool hot_soaked = false; ///< true when the sensor has achieved the specified temperature increase + bool tempcal_complete = false; ///< true when the calibration has been completed + bool has_valid_temperature = false; ///< true if this sensor has temperature sensor + float low_temp = 0.f; ///< low temperature recorded at start of calibration (deg C) + float high_temp = 0.f; ///< highest temperature recorded during calibration (deg C) + float ref_temp = 0.f; /**< calibration reference temperature, nominally in the middle of the + calibration temperature range (deg C) */ + }; + + PerSensorData _data[SENSOR_COUNT_MAX]; + + /** + * update a single sensor instance + * @return 0 when done, 1 not finished yet, <0 for an error + */ + virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0; + + unsigned _num_sensor_instances{0}; + int _sensor_subs[SENSOR_COUNT_MAX]; +}; diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/gyro.cpp b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/gyro.cpp new file mode 100644 index 0000000..22cb0f1 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/gyro.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 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 gyro.cpp + * Implementation of the Gyro Temperature Calibration for onboard sensors. + * + * @author Siddharth Bharat Purohit + * @author Beat Küng + */ + +#include +#include +#include "gyro.h" +#include + +TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, + float max_start_temperature, int gyro_subs[], int num_gyros) + : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) +{ + for (int i = 0; i < num_gyros; ++i) { + _sensor_subs[i] = gyro_subs[i]; + } + + _num_sensor_instances = num_gyros; +} + +int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub) +{ + bool finished = data.hot_soaked; + + bool updated; + orb_check(sensor_sub, &updated); + + if (!updated) { + return finished ? 0 : 1; + } + + sensor_gyro_s gyro_data{}; + orb_copy(ORB_ID(sensor_gyro), sensor_sub, &gyro_data); + + if (finished) { + // if we're done, return, but we need to return after orb_copy because of poll() + return 0; + } + + if (PX4_ISFINITE(gyro_data.temperature)) { + data.has_valid_temperature = true; + + } else { + return 0; + } + + data.device_id = gyro_data.device_id; + + data.sensor_sample_filt[0] = gyro_data.x; + data.sensor_sample_filt[1] = gyro_data.y; + data.sensor_sample_filt[2] = gyro_data.z; + data.sensor_sample_filt[3] = gyro_data.temperature; + + // wait for min start temp to be reached before starting calibration + if (data.sensor_sample_filt[3] < _min_start_temperature) { + return 1; + } + + if (!data.cold_soaked) { + // allow time for sensors and filters to settle + if (hrt_absolute_time() > 10E6) { + // If intial temperature exceeds maximum declare an error condition and exit + if (data.sensor_sample_filt[3] > _max_start_temperature) { + return -TC_ERROR_INITIAL_TEMP_TOO_HIGH; + + } else { + data.cold_soaked = true; + data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature + data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature + data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise; + return 1; + } + + } else { + return 1; + } + } + + // check if temperature increased + if (data.sensor_sample_filt[3] > data.high_temp) { + data.high_temp = data.sensor_sample_filt[3]; + data.hot_soak_sat = 0; + + } else { + return 1; + } + + // TODO: Detect when temperature has stopped rising for more than TBD seconds + if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) { + data.hot_soaked = true; + } + + if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor + TC_DEBUG("\nGyro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0], + (double)data.sensor_sample_filt[1], + (double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp, + (double)(data.high_temp - data.low_temp)); + } + + //update linear fit matrices + double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp; + data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]); + data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]); + data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]); + + return 1; +} + +int TemperatureCalibrationGyro::finish() +{ + for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { + finish_sensor_instance(_data[uorb_index], uorb_index); + } + + int32_t enabled = 1; + int result = param_set_no_notification(param_find("TC_G_ENABLE"), &enabled); + + if (result != PX4_OK) { + PX4_ERR("unable to reset TC_G_ENABLE (%i)", result); + } + + return result; +} + +int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int sensor_index) +{ + if (!data.has_valid_temperature) { + PX4_WARN("Result Gyro %d does not have a valid temperature sensor", sensor_index); + data.tempcal_complete = true; + + uint32_t param = 0; + set_parameter("TC_G%d_ID", sensor_index, ¶m); + return 0; + } + + if (!data.hot_soaked || data.tempcal_complete) { + return 0; + } + + double res[3][4] {}; + data.P[0].fit(res[0]); + PX4_INFO("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1], + (double)res[0][2], + (double)res[0][3]); + data.P[1].fit(res[1]); + PX4_INFO("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1], + (double)res[1][2], + (double)res[1][3]); + data.P[2].fit(res[2]); + PX4_INFO("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1], + (double)res[2][2], + (double)res[2][3]); + data.tempcal_complete = true; + + char str[30] {}; + float param = 0.0f; + int result = PX4_OK; + + set_parameter("TC_G%d_ID", sensor_index, &data.device_id); + + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + for (unsigned coef_index = 0; coef_index <= 3; coef_index++) { + sprintf(str, "TC_G%d_X%d_%d", sensor_index, 3 - coef_index, axis_index); + param = (float)res[axis_index][coef_index]; + result = param_set_no_notification(param_find(str), ¶m); + + if (result != PX4_OK) { + PX4_ERR("unable to reset %s", str); + } + } + } + + set_parameter("TC_G%d_TMAX", sensor_index, &data.high_temp); + set_parameter("TC_G%d_TMIN", sensor_index, &data.low_temp); + set_parameter("TC_G%d_TREF", sensor_index, &data.ref_temp); + + return 0; +} diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/gyro.h b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/gyro.h new file mode 100644 index 0000000..c6f9a5d --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/gyro.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include "common.h" +#include "polyfit.hpp" + +class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> +{ +public: + TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature, + int gyro_subs[], int num_gyros); + virtual ~TemperatureCalibrationGyro() {} + + /** + * @see TemperatureCalibrationBase::finish() + */ + int finish(); + +private: + + virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); + + inline int finish_sensor_instance(PerSensorData &data, int sensor_index); +}; diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/polyfit.hpp b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/polyfit.hpp new file mode 100644 index 0000000..70cc28a --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/polyfit.hpp @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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. + * + ****************************************************************************/ + +/* + +This algorithm performs a curve fit of m x,y data points using a polynomial +equation of the following form: + +yi = a0 + a1.xi + a2.xi^2 + a3.xi^3 + .... + an.xi^n + ei , where: + +i = [0,m] +xi is the x coordinate (independant variable) of the i'th measurement +yi is the y coordinate (dependant variable) of the i'th measurement +ei is a random fit error being the difference between the i'th y coordinate + and the value predicted by the polynomial. + +In vector form this is represented as: + +Y = V.A + E , where: + +V is Vandermonde matrix in x -> https://en.wikipedia.org/wiki/Vandermonde_matrix +Y is a vector of length m containing the y measurements +E is a vector of length m containing the fit errors for each measurement + +Use an Ordinary Least Squares derivation to minimise ∑(i=0..m)ei^2 -> https://en.wikipedia.org/wiki/Ordinary_least_squares + +Note: In the wikipedia reference, the X matrix in reference is equivalent to our V matrix and the Beta matrix is equivalent to our A matrix + +A = inv(transpose(V)*V)*(transpose(V)*Y) + +We can accumulate VTV and VTY recursively as they are of fixed size, where: + +VTV = transpose(V)*V = + __ __ +| m+1 x0+x1+...+xm x0^2+x1^2+...+xm^3 .......... x0^n+x1^n+...+xm^n | +|x0+x1+...+xm x0^2+x1^2+...+xm^3 x0^3+x1^3+...+xm^3 .......... x0^(n+1)+x1^(n+1)+...+xm^(n+1) | +| . . . . | +| . . . . | +| . . . . | +|x0^n+x1^n+...+xm^n x0^(n+1)+x1^(n+1)+...+xm^(n+1) x0^(n+2)+x1^(n+2)+...+xm^(n+2) .... x0^(2n)+x1^(2n)+...+xm^(2n) | +|__ __| + +and VTY = transpose(V)*Y = + __ __ +| ∑(i=0..m)yi | +| ∑(i=0..m)yi*xi | +| . | +| . | +| . | +|∑(i=0..m)yi*xi^n| +|__ __| + +*/ + +/* +Polygon linear fit +Author: Siddharth Bharat Purohit +*/ + +#pragma once +#include +#include +#include +#include +#include + +#include + +#include + +#define DEBUG 0 +#if DEBUG +#define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__); +#else +#define PF_DEBUG(fmt, ...) +#endif + +template +class polyfitter +{ +public: + polyfitter() {} + + void update(double x, double y) + { + update_VTV(x); + update_VTY(x, y); + } + + bool fit(double res[]) + { + //Do inverse of VTV + matrix::SquareMatrix IVTV; + + IVTV = _VTV.I(); + + for (int i = 0; i < _forder; i++) { + for (int j = 0; j < _forder; j++) { + PF_DEBUG("%.10f ", (double)IVTV(i, j)); + } + + PF_DEBUG("\n"); + } + + for (int i = 0; i < _forder; i++) { + res[i] = 0.0; + + for (int j = 0; j < _forder; j++) { + res[i] += IVTV(i, j) * (double)_VTY(j); + } + + PF_DEBUG("%.10f ", res[i]); + } + + return true; + } + +private: + matrix::SquareMatrix _VTV; + matrix::Vector _VTY; + + void update_VTY(double x, double y) + { + double temp = 1.0; + PF_DEBUG("O %.6f\n", (double)x); + + for (int i = _forder - 1; i >= 0; i--) { + _VTY(i) += y * temp; + temp *= x; + PF_DEBUG("%.6f ", (double)_VTY(i)); + } + + PF_DEBUG("\n"); + } + + void update_VTV(double x) + { + double temp = 1.0; + int8_t z; + + for (int i = 0; i < _forder; i++) { + for (int j = 0; j < _forder; j++) { + PF_DEBUG("%.10f ", (double)_VTV(i, j)); + } + + PF_DEBUG("\n"); + } + + for (int i = 2 * _forder - 2; i >= 0; i--) { + if (i < _forder) { + z = 0.0f; + + } else { + z = i - _forder + 1; + } + + for (int j = i - z; j >= z; j--) { + int row = j; + int col = i - j; + _VTV(row, col) += (double)temp; + } + + temp *= x; + } + } +}; diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/task.cpp new file mode 100644 index 0000000..cb5d011 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -0,0 +1,352 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file task.cpp + * + * Main task handling the temperature calibration process + * + * @author Beat Küng + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "common.h" +#include "temperature_calibration.h" +#include "accel.h" +#include "baro.h" +#include "gyro.h" + +class TemperatureCalibration; + +namespace temperature_calibration +{ +px4::atomic instance{nullptr}; +} + +class TemperatureCalibration +{ +public: + + TemperatureCalibration(bool accel, bool baro, bool gyro) : _accel(accel), _baro(baro), _gyro(gyro) {} + ~TemperatureCalibration() = default; + + /** + * Start task. + * + * @return OK on success. + */ + int start(); + + static int do_temperature_calibration(int argc, char *argv[]); + + void task_main(); + + void exit_task() { _force_task_exit = true; } + +private: + void publish_led_control(led_control_s &led_control); + + uORB::Publication _led_control_pub{ORB_ID(led_control)}; + + bool _force_task_exit = false; + int _control_task = -1; // task handle for task + + const bool _accel; ///< enable accel calibration? + const bool _baro; ///< enable baro calibration? + const bool _gyro; ///< enable gyro calibration? +}; + +void TemperatureCalibration::task_main() +{ + // subscribe to all gyro instances + int gyro_sub[SENSOR_COUNT_MAX] {-1, -1, -1}; + px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] {}; + unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro)); + + if (num_gyro > SENSOR_COUNT_MAX) { + num_gyro = SENSOR_COUNT_MAX; + } + + for (unsigned i = 0; i < num_gyro; i++) { + gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i); + fds[i].fd = gyro_sub[i]; + fds[i].events = POLLIN; + } + + int32_t min_temp_rise = 24; + param_get(param_find("SYS_CAL_TDEL"), &min_temp_rise); + PX4_INFO("Waiting for %" PRId32 " degrees difference in sensor temperature", min_temp_rise); + + int32_t min_start_temp = 5; + param_get(param_find("SYS_CAL_TMIN"), &min_start_temp); + + int32_t max_start_temp = 10; + param_get(param_find("SYS_CAL_TMAX"), &max_start_temp); + + //init calibrators + TemperatureCalibrationBase *calibrators[3] {}; + bool error_reported[3] {}; + int num_calibrators = 0; + + if (_accel) { + calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise, min_start_temp, max_start_temp); + + if (calibrators[num_calibrators]) { + ++num_calibrators; + + } else { + PX4_ERR("alloc failed"); + } + } + + if (_baro) { + calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise, min_start_temp, max_start_temp); + + if (calibrators[num_calibrators]) { + ++num_calibrators; + + } else { + PX4_ERR("alloc failed"); + } + } + + if (_gyro) { + calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_start_temp, max_start_temp, gyro_sub, + num_gyro); + + if (calibrators[num_calibrators]) { + ++num_calibrators; + + } else { + PX4_ERR("alloc failed"); + } + } + + hrt_abstime next_progress_output = hrt_absolute_time() + 1e6; + + // control LED's: blink, then turn solid according to progress + led_control_s led_control{}; + led_control.led_mask = 0xff; + led_control.mode = led_control_s::MODE_BLINK_NORMAL; + led_control.priority = led_control_s::MAX_PRIORITY; + led_control.color = led_control_s::COLOR_YELLOW; + led_control.num_blinks = 0; + publish_led_control(led_control); + int leds_completed = 0; + + bool abort_calibration = false; + + while (!_force_task_exit) { + /* we poll on the gyro(s), since this is the sensor with the highest update rate. + * Each individual sensor will then check on its own if there's new data. + */ + int ret = px4_poll(fds, num_gyro, 1000); + + if (ret < 0) { + // Poll error, sleep and try again + px4_usleep(10000); + continue; + + } else if (ret == 0) { + // Poll timeout or no new data, do nothing + continue; + } + + //if gyro is not enabled: we must do an orb_copy here, so that poll() does not immediately return again + if (!_gyro) { + sensor_gyro_s gyro_data; + + for (unsigned i = 0; i < num_gyro; ++i) { + orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data); + } + } + + int min_progress = 110; + + for (int i = 0; i < num_calibrators; ++i) { + ret = calibrators[i]->update(); + + if (ret == -TC_ERROR_COMMUNICATION) { + abort_calibration = true; + PX4_ERR("Calibration won't start - sensor bad or communication error"); + _force_task_exit = true; + break; + + } else if (ret == -TC_ERROR_INITIAL_TEMP_TOO_HIGH) { + abort_calibration = true; + PX4_ERR("Calibration won't start - sensor temperature too high"); + _force_task_exit = true; + break; + + } else if (ret < 0 && !error_reported[i]) { + // temperature has decreased so calibration is not being updated + error_reported[i] = true; + PX4_ERR("Calibration update step failed (%i)", ret); + + } else if (ret < min_progress) { + // temperature is stable or increasing + min_progress = ret; + } + } + + if (min_progress == 110 || abort_calibration) { + break; // we are done + } + + int led_progress = min_progress * BOARD_MAX_LEDS / 100; + + for (; leds_completed < led_progress; ++leds_completed) { + led_control.led_mask = 1 << leds_completed; + led_control.mode = led_control_s::MODE_ON; + publish_led_control(led_control); + } + + //print progress each second + hrt_abstime now = hrt_absolute_time(); + + if (now > next_progress_output) { + PX4_INFO("Calibration progress: %i%%", min_progress); + next_progress_output = now + 1e6; + } + } + + if (abort_calibration) { + led_control.color = led_control_s::COLOR_RED; + + } else { + PX4_INFO("Sensor Measurments completed"); + + // save params immediately so that we can check the result and don't have to wait for param save timeout + param_control_autosave(false); + + // do final calculations & parameter storage + for (int i = 0; i < num_calibrators; ++i) { + int ret = calibrators[i]->finish(); + + if (ret < 0) { + PX4_ERR("Failed to finish calibration process (%i)", ret); + } + } + + param_notify_changes(); + int ret = param_save_default(); + + if (ret != 0) { + PX4_ERR("Failed to save params (%i)", ret); + } + + param_control_autosave(true); + + led_control.color = led_control_s::COLOR_GREEN; + } + + // blink the LED's according to success/failure + led_control.led_mask = 0xff; + led_control.mode = led_control_s::MODE_BLINK_FAST; + led_control.num_blinks = 0; + publish_led_control(led_control); + + for (int i = 0; i < num_calibrators; ++i) { + delete calibrators[i]; + } + + for (unsigned i = 0; i < num_gyro; i++) { + orb_unsubscribe(gyro_sub[i]); + } + + delete temperature_calibration::instance.load(); + temperature_calibration::instance.store(nullptr); + PX4_INFO("Exiting temperature calibration task"); +} + +int TemperatureCalibration::do_temperature_calibration(int argc, char *argv[]) +{ + temperature_calibration::instance.load()->task_main(); + return 0; +} + +int TemperatureCalibration::start() +{ + _control_task = px4_task_spawn_cmd("temperature_calib", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 5800, + (px4_main_t)&TemperatureCalibration::do_temperature_calibration, + nullptr); + + if (_control_task < 0) { + delete temperature_calibration::instance.load(); + temperature_calibration::instance.store(nullptr); + PX4_ERR("start failed"); + return -errno; + } + + return 0; +} + +void TemperatureCalibration::publish_led_control(led_control_s &led_control) +{ + led_control.timestamp = hrt_absolute_time(); + _led_control_pub.publish(led_control); +} + +int run_temperature_calibration(bool accel, bool baro, bool gyro) +{ + if (temperature_calibration::instance.load() == nullptr) { + PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro); + temperature_calibration::instance.store(new TemperatureCalibration(accel, baro, gyro)); + + if (temperature_calibration::instance.load() == nullptr) { + PX4_ERR("alloc failed"); + return 1; + } + + return temperature_calibration::instance.load()->start(); + + } else { + PX4_WARN("temperature calibration task already running"); + } + + return PX4_ERROR; +} diff --git a/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h new file mode 100644 index 0000000..d5a7bd2 --- /dev/null +++ b/src/prometheus_px4/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + + +/** start temperature calibration in a new task for one or multiple sensors + * @return 0 on success, <0 error otherwise */ +int run_temperature_calibration(bool accel, bool baro, bool gyro); diff --git a/src/prometheus_px4/src/modules/uuv_att_control/CMakeLists.txt b/src/prometheus_px4/src/modules/uuv_att_control/CMakeLists.txt new file mode 100644 index 0000000..37dc0d7 --- /dev/null +++ b/src/prometheus_px4/src/modules/uuv_att_control/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE modules__uuv_att_control + MAIN uuv_att_control + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + uuv_att_control.cpp + ) diff --git a/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control.cpp b/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control.cpp new file mode 100644 index 0000000..719f4c5 --- /dev/null +++ b/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control.cpp @@ -0,0 +1,369 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the fixed wing / rover module and it is designed for unmanned underwater vehicles (UUV). + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the acknowledgments and credits for the fw wing/rover app are reported in those files. + * + * @author Daniel Duecker + * @author Philipp Hastedt + * @author Tim Hansen + */ + +#include "uuv_att_control.hpp" + + +#define ACTUATOR_PUBLISH_PERIOD_MS 4 + + +/** + * UUV attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int uuv_att_control_main(int argc, char *argv[]); + + +UUVAttitudeControl::UUVAttitudeControl(): + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ +} + +UUVAttitudeControl::~UUVAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool UUVAttitudeControl::init() +{ + if (!_vehicle_attitude_sub.registerCallback()) { + PX4_ERR("vehicle_attitude callback registration failed!"); + return false; + } + + return true; +} + +void UUVAttitudeControl::parameters_update(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(); + } +} + +void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, + float thrust_x, float thrust_y, float thrust_z) +{ + if (PX4_ISFINITE(roll_u)) { + roll_u = math::constrain(roll_u, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_ROLL] = roll_u; + + } else { + _actuators.control[actuator_controls_s::INDEX_ROLL] = 0.0f; + } + + if (PX4_ISFINITE(pitch_u)) { + pitch_u = math::constrain(pitch_u, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_PITCH] = pitch_u; + + } else { + _actuators.control[actuator_controls_s::INDEX_PITCH] = 0.0f; + } + + if (PX4_ISFINITE(yaw_u)) { + yaw_u = math::constrain(yaw_u, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_YAW] = yaw_u; + + } else { + _actuators.control[actuator_controls_s::INDEX_YAW] = 0.0f; + } + + if (PX4_ISFINITE(thrust_x)) { + thrust_x = math::constrain(thrust_x, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = thrust_x; + + } else { + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + } + + if (PX4_ISFINITE(thrust_y)) { + thrust_y = math::constrain(thrust_y, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_FLAPS] = thrust_y; + + } else { + _actuators.control[actuator_controls_s::INDEX_FLAPS] = 0.0f; + } + + if (PX4_ISFINITE(thrust_z)) { + thrust_z = math::constrain(thrust_z, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_SPOILERS] = thrust_z; + + } else { + _actuators.control[actuator_controls_s::INDEX_SPOILERS] = 0.0f; + } +} + +void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude, + const vehicle_attitude_setpoint_s &attitude_setpoint, const vehicle_angular_velocity_s &angular_velocity, + const vehicle_rates_setpoint_s &rates_setpoint) +{ + /** Geometric Controller + * + * based on + * D. Mellinger, V. Kumar, "Minimum Snap Trajectory Generation and Control for Quadrotors", IEEE ICRA 2011, pp. 2520-2525. + * D. A. Duecker, A. Hackbarth, T. Johannink, E. Kreuzer, and E. Solowjow, “Micro Underwater Vehicle Hydrobatics: A SubmergedFuruta Pendulum,” IEEE ICRA 2018, pp. 7498–7503. + */ + Eulerf euler_angles(matrix::Quatf(attitude.q)); + + float roll_u; + float pitch_u; + float yaw_u; + float thrust_x; + float thrust_y; + float thrust_z; + + float roll_body = attitude_setpoint.roll_body; + float pitch_body = attitude_setpoint.pitch_body; + float yaw_body = attitude_setpoint.yaw_body; + + float roll_rate_desired = rates_setpoint.roll; + float pitch_rate_desired = rates_setpoint.pitch; + float yaw_rate_desired = rates_setpoint.yaw; + + /* get attitude setpoint rotational matrix */ + Dcmf rot_des = Eulerf(roll_body, pitch_body, yaw_body); + + /* get current rotation matrix from control state quaternions */ + Quatf q_att(attitude.q); + Matrix3f rot_att = matrix::Dcm(q_att); + + Vector3f e_R_vec; + Vector3f torques; + + /* Compute matrix: attitude error */ + Matrix3f e_R = (rot_des.transpose() * rot_att - rot_att.transpose() * rot_des) * 0.5; + + /* vee-map the error to get a vector instead of matrix e_R */ + e_R_vec(0) = e_R(2, 1); /**< Roll */ + e_R_vec(1) = e_R(0, 2); /**< Pitch */ + e_R_vec(2) = e_R(1, 0); /**< Yaw */ + + Vector3f omega{angular_velocity.xyz}; + omega(0) -= roll_rate_desired; + omega(1) -= pitch_rate_desired; + omega(2) -= yaw_rate_desired; + + /**< P-Control */ + torques(0) = - e_R_vec(0) * _param_roll_p.get(); /**< Roll */ + torques(1) = - e_R_vec(1) * _param_pitch_p.get(); /**< Pitch */ + torques(2) = - e_R_vec(2) * _param_yaw_p.get(); /**< Yaw */ + + /**< PD-Control */ + torques(0) = torques(0) - omega(0) * _param_roll_d.get(); /**< Roll */ + torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */ + torques(2) = torques(2) - omega(2) * _param_yaw_d.get(); /**< Yaw */ + + roll_u = torques(0); + pitch_u = torques(1); + yaw_u = torques(2); + + // take thrust as + thrust_x = attitude_setpoint.thrust_body[0]; + thrust_y = attitude_setpoint.thrust_body[1]; + thrust_z = attitude_setpoint.thrust_body[2]; + + + constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z); + /* Geometric Controller END*/ +} + +void UUVAttitudeControl::Run() +{ + if (should_exit()) { + _vehicle_attitude_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + /* check vehicle control mode for changes to publication state */ + _vcontrol_mode_sub.update(&_vcontrol_mode); + + /* update parameters from storage */ + parameters_update(); + + vehicle_attitude_s attitude; + + /* only run controller if attitude changed */ + if (_vehicle_attitude_sub.update(&attitude)) { + vehicle_angular_velocity_s angular_velocity {}; + _angular_velocity_sub.copy(&angular_velocity); + + /* Run geometric attitude controllers if NOT manual mode*/ + if (!_vcontrol_mode.flag_control_manual_enabled + && _vcontrol_mode.flag_control_attitude_enabled + && _vcontrol_mode.flag_control_rates_enabled) { + + int input_mode = _param_input_mode.get(); + + _vehicle_attitude_setpoint_sub.update(&_attitude_setpoint); + _vehicle_rates_setpoint_sub.update(&_rates_setpoint); + + if (input_mode == 1) { // process manual data + _attitude_setpoint.roll_body = _param_direct_roll.get(); + _attitude_setpoint.pitch_body = _param_direct_pitch.get(); + _attitude_setpoint.yaw_body = _param_direct_yaw.get(); + _attitude_setpoint.thrust_body[0] = _param_direct_thrust.get(); + _attitude_setpoint.thrust_body[1] = 0.f; + _attitude_setpoint.thrust_body[2] = 0.f; + } + + /* Geometric Control*/ + int skip_controller = _param_skip_ctrl.get(); + + if (skip_controller) { + constrain_actuator_commands(_rates_setpoint.roll, _rates_setpoint.pitch, _rates_setpoint.yaw, + _rates_setpoint.thrust_body[0], _rates_setpoint.thrust_body[1], _rates_setpoint.thrust_body[2]); + + } else { + control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint); + } + } + } + + /* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */ + if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) { + // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep + // returning immediately and this loop will eat up resources. + if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { + /* manual/direct control */ + constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x, + _manual_control_setpoint.r, + _manual_control_setpoint.z, 0.f, 0.f); + } + + } + + _actuators.timestamp = hrt_absolute_time(); + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_manual_enabled || + _vcontrol_mode.flag_control_attitude_enabled) { + /* publish the actuator controls */ + _actuator_controls_pub.publish(_actuators); + + } + + perf_end(_loop_perf); +} + +int UUVAttitudeControl::task_spawn(int argc, char *argv[]) +{ + UUVAttitudeControl *instance = new UUVAttitudeControl(); + + 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 UUVAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + + +int UUVAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Controls the attitude of an unmanned underwater vehicle (UUV). + +Publishes `actuator_controls_0` messages at a constant 250Hz. + +### Implementation +Currently, this implementation supports only a few modes: + + * Full manual: Roll, pitch, yaw, and throttle controls are passed directly through to the actuators + * Auto mission: The uuv runs missions + +### Examples +CLI usage example: +$ uuv_att_control start +$ uuv_att_control status +$ uuv_att_control stop + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("uuv_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start") + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int uuv_att_control_main(int argc, char *argv[]) +{ + return UUVAttitudeControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control.hpp b/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control.hpp new file mode 100644 index 0000000..6bccfea --- /dev/null +++ b/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control.hpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the rover attitide control module and is designed for the + * TUHH hippocampus. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Daniel Duecker + * @author Philipp Hastedt + * @author Tim Hansen + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Matrix3f; +using matrix::Vector3f; +using matrix::Dcmf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +class UUVAttitudeControl: public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + UUVAttitudeControl(); + ~UUVAttitudeControl(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */ + uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle bodyrates setpoint subscriber */ + uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; /**< vehicle angular velocity subscription */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ + + uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; + + actuator_controls_s _actuators {}; /**< actuator control inputs */ + manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ + vehicle_attitude_setpoint_s _attitude_setpoint {}; /**< vehicle attitude setpoint */ + vehicle_rates_setpoint_s _rates_setpoint {}; /**< vehicle bodyrates setpoint */ + vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_roll_p, + (ParamFloat) _param_roll_d, + (ParamFloat) _param_pitch_p, + (ParamFloat) _param_pitch_d, + (ParamFloat) _param_yaw_p, + (ParamFloat) _param_yaw_d, + // control/input modes + (ParamInt) _param_input_mode, + (ParamInt) _param_skip_ctrl, + // direct access to inputs + (ParamFloat) _param_direct_roll, + (ParamFloat) _param_direct_pitch, + (ParamFloat) _param_direct_yaw, + (ParamFloat) _param_direct_thrust + ) + + void Run() override; + /** + * Update our local parameter cache. + */ + void parameters_update(bool force = false); + + /** + * Control Attitude + */ + void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint, + const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint); + void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, + float thrust_x, float thrust_y, float thrust_z); +}; diff --git a/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control_params.c b/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control_params.c new file mode 100644 index 0000000..fdad0b3 --- /dev/null +++ b/src/prometheus_px4/src/modules/uuv_att_control/uuv_att_control_params.c @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * 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 uuv_att_control_params.c + * + * Parameters defined by the attitude control task for unmanned underwater vehicles (UUVs) + * + * This is a modification of the fixed wing/ground rover params and it is designed for ground rovers. + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the ackowledgments and credits for the fw wing/rover app are reported in those files. + * + * @author Daniel Duecker + */ + +/* + * Controller parameters, accessible via MAVLink + * + */ + +// Roll gains +/** + * Roll proportional gain + * + * @group UUV Attitude Control + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_ROLL_P, 4.0f); + +/** + * Roll differential gain + * + * @group UUV Attitude Control + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_ROLL_D, 1.5f); + + +// Pitch gains +/** + * Pitch proportional gain + * + * @group UUV Attitude Control + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_PITCH_P, 4.0f); + +/** + * Pitch differential gain + * + * @group UUV Attitude Control + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_PITCH_D, 2.0f); + + +// Yaw gains +/** + * Yawh proportional gain + * + * @group UUV Attitude Control + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_YAW_P, 4.0f); + +/** + * Yaw differential gain + * + * @group UUV Attitude Control + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(UUV_YAW_D, 2.0f); + + +// Input Modes +/** + * Select Input Mode + * + * @value 0 use Attitude Setpoints + * @value 1 Direct Feedthrough + * @group UUV Attitude Control + */ +PARAM_DEFINE_INT32(UUV_INPUT_MODE, 0); + +/** + * Skip the controller + * + * @value 0 use the module's controller + * @value 1 skip the controller and feedthrough the setpoints + */ +PARAM_DEFINE_INT32(UUV_SKIP_CTRL, 0); + +/** + * Direct roll input + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_DIRCT_ROLL, 0.0f); + +/** + * Direct pitch input + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_DIRCT_PITCH, 0.0f); + +/** + * Direct yaw input + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_DIRCT_YAW, 0.0f); + +/** + * Direct thrust input + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_DIRCT_THRUST, 0.0f); diff --git a/src/prometheus_px4/src/modules/uuv_pos_control/CMakeLists.txt b/src/prometheus_px4/src/modules/uuv_pos_control/CMakeLists.txt new file mode 100644 index 0000000..f97dd0e --- /dev/null +++ b/src/prometheus_px4/src/modules/uuv_pos_control/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE modules__uuv_pos_control + MAIN uuv_pos_control + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + uuv_pos_control.cpp + ) diff --git a/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control.cpp new file mode 100644 index 0000000..37f277d --- /dev/null +++ b/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the hippocampus control module and is designed for the + * BlueROV2. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Tim Hansen + * @author Daniel Duecker + */ + +#include "uuv_pos_control.hpp" + + + +/** + * UUV pos_controller app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int uuv_pos_control_main(int argc, char *argv[]); + + +UUVPOSControl::UUVPOSControl(): + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ +} + +UUVPOSControl::~UUVPOSControl() +{ + perf_free(_loop_perf); +} + +bool UUVPOSControl::init() +{ + if (!_vehicle_local_position_sub.registerCallback()) { + PX4_ERR("vehicle_pos callback registration failed!"); + return false; + } + + return true; +} + +void UUVPOSControl::parameters_update(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(); + } +} + +void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z, + const float roll_des, const float pitch_des, const float yaw_des) +{ + //watch if inputs are not to high + vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {}; + vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + + vehicle_attitude_setpoint.roll_body = roll_des; + vehicle_attitude_setpoint.pitch_body = pitch_des; + vehicle_attitude_setpoint.yaw_body = yaw_des; + + vehicle_attitude_setpoint.thrust_body[0] = thrust_x; + vehicle_attitude_setpoint.thrust_body[1] = thrust_y; + vehicle_attitude_setpoint.thrust_body[2] = thrust_z; + + + _att_sp_pub.publish(vehicle_attitude_setpoint); +} + +void UUVPOSControl::pose_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos) +{ + //get current rotation of vehicle + Quatf q_att(vehicle_attitude.q); + Vector3f pos_des = Vector3f(x_pos_des, y_pos_des, z_pos_des); + + Vector3f p_control_output = Vector3f(_param_pose_gain_x.get() * (pos_des(0) - vlocal_pos.x) - _param_pose_gain_d_x.get() + * vlocal_pos.vx, + _param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy, + _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz); + + Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body) + + publish_attitude_setpoint(rotated_input(0), + rotated_input(1), + rotated_input(2), + roll_des, pitch_des, yaw_des); + +} + +void UUVPOSControl::stabilization_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos) +{ + //get current rotation of vehicle + Quatf q_att(vehicle_attitude.q); + Vector3f pos_des = Vector3f(0, 0, z_pos_des); + + Vector3f p_control_output = Vector3f(0, + 0, + _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z)); + //potential d controller missing + Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body) + + publish_attitude_setpoint(rotated_input(0) + x_pos_des, rotated_input(1) + y_pos_des, rotated_input(2), + roll_des, pitch_des, yaw_des); + +} + +void UUVPOSControl::Run() +{ + if (should_exit()) { + _vehicle_local_position_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + /* check vehicle control mode for changes to publication state */ + _vcontrol_mode_sub.update(&_vcontrol_mode); + + + /* update parameters from storage */ + parameters_update(); + + //vehicle_attitude_s attitude; + vehicle_local_position_s vlocal_pos; + + /* only run controller if local_pos changed */ + if (_vehicle_local_position_sub.update(&vlocal_pos)) { + + /* Run geometric attitude controllers if NOT manual mode*/ + if (!_vcontrol_mode.flag_control_manual_enabled + && _vcontrol_mode.flag_control_attitude_enabled + && _vcontrol_mode.flag_control_rates_enabled) { + + _vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude + _trajectory_setpoint_sub.update(&_trajectory_setpoint); + + float roll_des = 0; + float pitch_des = 0; + float yaw_des = _trajectory_setpoint.yaw; + + float x_pos_des = _trajectory_setpoint.x; + float y_pos_des = _trajectory_setpoint.y; + float z_pos_des = _trajectory_setpoint.z; + + //stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw) + if (_param_stabilization.get() == 0) { + pose_controller_6dof(x_pos_des, y_pos_des, z_pos_des, + roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); + + } else { + stabilization_controller_6dof(x_pos_des, y_pos_des, z_pos_des, + roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); + } + } + } + + /* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */ + if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) { + // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep + // returning immediately and this loop will eat up resources. + if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { + /* manual/direct control */ + } + + } + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_manual_enabled || + _vcontrol_mode.flag_control_attitude_enabled) { + } + + perf_end(_loop_perf); +} + +int UUVPOSControl::task_spawn(int argc, char *argv[]) +{ + UUVPOSControl *instance = new UUVPOSControl(); + + 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 UUVPOSControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + + +int UUVPOSControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Controls the attitude of an unmanned underwater vehicle (UUV). +Publishes `actuator_controls_0` messages at a constant 250Hz. +### Implementation +Currently, this implementation supports only a few modes: + * Full manual: Roll, pitch, yaw, and throttle controls are passed directly through to the actuators + * Auto mission: The uuv runs missions +### Examples +CLI usage example: +$ uuv_pos_control start +$ uuv_pos_control status +$ uuv_pos_control stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("uuv_pos_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start") + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int uuv_pos_control_main(int argc, char *argv[]) +{ + return UUVPOSControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control.hpp new file mode 100644 index 0000000..e78acc0 --- /dev/null +++ b/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the hippocampus control module and is designed for the + * BlueROV2. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Tim Hansen + * @author Daniel Duecker + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Matrix3f; +using matrix::Vector3f; +using matrix::Dcmf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +class UUVPOSControl: public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + UUVPOSControl(); + ~UUVPOSControl(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; /**< current vehicle attitude */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ + + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; + + //actuator_controls_s _actuators {}; /**< actuator control inputs */ + manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ + vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */ + vehicle_local_position_setpoint_s _trajectory_setpoint{}; /**< vehicle position setpoint */ + vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_pose_gain_x, + (ParamFloat) _param_pose_gain_y, + (ParamFloat) _param_pose_gain_z, + (ParamFloat) _param_pose_gain_d_x, + (ParamFloat) _param_pose_gain_d_y, + (ParamFloat) _param_pose_gain_d_z, + + (ParamInt) _param_input_mode, + (ParamInt) _param_stabilization, + (ParamInt) _param_skip_ctrl + ) + + void Run() override; + /** + * Update our local parameter cache. + */ + void parameters_update(bool force = false); + + /** + * Control Attitude + */ + void publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z, + const float roll_des, const float pitch_des, const float yaw_des); + void pose_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos); + void stabilization_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos); +}; diff --git a/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control_params.c b/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control_params.c new file mode 100644 index 0000000..d3532ed --- /dev/null +++ b/src/prometheus_px4/src/modules/uuv_pos_control/uuv_pos_control_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 uuv_pos_control_params.c + * + * Parameters defined by the position control task for unmanned underwater vehicles (UUVs) + * + * This is a modification of the fixed wing/ground rover params and it is designed for ground rovers. + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the ackowledgments and credits for the fw wing/rover app are reported in those files. + * + * @author Tim Hansen + * @author Daniel Duecker + */ + +/* + * Controller parameters, accessible via MAVLink + * + */ +/** + * Gain of P controller X + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_X_P, 1.0f); +/** + * Gain of P controller Y + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Y_P, 1.0f); +/** + * Gain of P controller Z + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Z_P, 1.0f); + +/** + * Gain of D controller X + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_X_D, 0.2f); +/** + * Gain of D controller Y + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Y_D, 0.2f); +/** + * Gain of D controller Z + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Z_D, 0.2f); + +/** + * Stabilization mode(1) or Position Control(0) + * + * @value 0 Position Control + * @value 1 Stabilization Mode + * @group UUV Position Control + */ +PARAM_DEFINE_INT32(UUV_STAB_MODE, 1); diff --git a/src/prometheus_px4/src/modules/vmount/CMakeLists.txt b/src/prometheus_px4/src/modules/vmount/CMakeLists.txt new file mode 100644 index 0000000..069a1cc --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ +px4_add_module( + MODULE drivers__vmount + MAIN vmount + COMPILE_FLAGS + SRCS + input.cpp + input_mavlink.cpp + input_rc.cpp + input_test.cpp + output.cpp + output_mavlink.cpp + output_rc.cpp + vmount.cpp + DEPENDS + git_ecl + ecl_geo + ) + diff --git a/src/prometheus_px4/src/modules/vmount/common.h b/src/prometheus_px4/src/modules/vmount/common.h new file mode 100644 index 0000000..d4ecfd8 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/common.h @@ -0,0 +1,95 @@ +/**************************************************************************** +* +* Copyright (c) 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 common.h + * @author Beat Küng + * + */ + +#pragma once + +#include + +namespace vmount +{ + +/** + * @struct ControlData + * This defines the common API between an input and an output of the vmount driver. + * Each output must support the (full) set of the commands, and an input can create all + * or a subset of the types. + */ +struct ControlData { + + enum class Type : uint8_t { + Neutral = 0, /**< move to neutral position */ + Angle, /**< control the roll, pitch & yaw angle directly */ + LonLat /**< control via lon, lat */ + }; + + + Type type = Type::Neutral; + + union TypeData { + struct TypeAngle { + float q[4]; /**< attitude quaternion */ + float angular_velocity[3]; // angular velocity + + // according to DO_MOUNT_CONFIGURE + enum class Frame : uint8_t { + AngleBodyFrame = 0, /**< Follow mode, angle relative to vehicle (usually default for yaw axis). */ + AngularRate = 1, /**< Angular rate set only, for compatibility with MAVLink v1 protocol. */ + AngleAbsoluteFrame = 2/**< Lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch). */ + } frames[3]; /**< Mode. */ + } angle; + + struct TypeLonLat { + double lon; /**< longitude in [deg] */ + double lat; /**< latitude in [deg] */ + float altitude; /**< altitude in [m] */ + float roll_angle; /**< roll is set to a fixed angle. Set to 0 for level horizon [rad] */ + float pitch_angle_offset; /**< angular offset for pitch [rad] */ + float yaw_angle_offset; /**< angular offset for yaw [rad] */ + float pitch_fixed_angle; /**< ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude */ + } lonlat; + } type_data; + + + bool stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis + (if the output supports it, this can also be done externally) */ + bool gimbal_shutter_retract = false; /**< whether to lock the gimbal (only in RC output mode) */ + +}; + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/input.cpp b/src/prometheus_px4/src/modules/vmount/input.cpp new file mode 100644 index 0000000..f6e5440 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/input.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 input.cpp + * @author Beat Küng + * + */ + +#include "input.h" + + +namespace vmount +{ + +int InputBase::update(unsigned int timeout_ms, ControlData **control_data, bool already_active) +{ + if (!_initialized) { + int ret = initialize(); + + if (ret) { + return ret; + } + + //on startup, set the mount to a neutral position + _control_data.type = ControlData::Type::Neutral; + _control_data.gimbal_shutter_retract = true; + *control_data = &_control_data; + _initialized = true; + return 0; + } + + return update_impl(timeout_ms, control_data, already_active); +} + +void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle, + float pitch_fixed_angle) +{ + _control_data.type = ControlData::Type::LonLat; + _control_data.type_data.lonlat.lon = lon; + _control_data.type_data.lonlat.lat = lat; + _control_data.type_data.lonlat.altitude = altitude; + _control_data.type_data.lonlat.roll_angle = roll_angle; + _control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle; + _control_data.type_data.lonlat.pitch_angle_offset = 0.f; + _control_data.type_data.lonlat.yaw_angle_offset = 0.f; +} + +void InputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize) +{ + _control_data.stabilize_axis[0] = roll_stabilize; + _control_data.stabilize_axis[1] = pitch_stabilize; + _control_data.stabilize_axis[2] = yaw_stabilize; +} + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/input.h b/src/prometheus_px4/src/modules/vmount/input.h new file mode 100644 index 0000000..e7b26a6 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/input.h @@ -0,0 +1,93 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 input.h + * @author Beat Küng + * + */ + +#pragma once + +#include "common.h" + +namespace vmount +{ + + +/** + ** class InputBase + * Base class for all driver input classes + */ +class InputBase +{ +public: + InputBase() = default; + virtual ~InputBase() = default; + + /** + * Wait for an input update, with a timeout. + * @param timeout_ms timeout in ms + * @param control_data unchanged on error. On success it is nullptr if no new + * data is available, otherwise set to an object. + * If it is set, the returned object will not be changed for + * subsequent calls to update() that return no new data + * (in other words: if (some) control_data values change, + * non-null will be returned). + * @param already_active true if the mode was already active last time, false if it was not and "major" + * change is necessary such as big stick movement for RC. + * @return 0 on success, <0 otherwise + */ + virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active); + + /** report status to stdout */ + virtual void print_status() = 0; + + void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize); + +protected: + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) = 0; + + virtual int initialize() { return 0; } + + void control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle = 0.f, + float pitch_fixed_angle = -10.f); + + ControlData _control_data; + +private: + bool _initialized = false; +}; + + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/input_mavlink.cpp b/src/prometheus_px4/src/modules/vmount/input_mavlink.cpp new file mode 100644 index 0000000..1c0fb12 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/input_mavlink.cpp @@ -0,0 +1,908 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 input_mavlink.cpp + * @author Leon Müller (thedevleon) + * @author Beat Küng + * + */ + +#include "input_mavlink.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vmount +{ + +InputMavlinkROI::~InputMavlinkROI() +{ + if (_vehicle_roi_sub >= 0) { + orb_unsubscribe(_vehicle_roi_sub); + } + + if (_position_setpoint_triplet_sub >= 0) { + orb_unsubscribe(_position_setpoint_triplet_sub); + } +} + +int InputMavlinkROI::initialize() +{ + _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); + + if (_vehicle_roi_sub < 0) { + return -errno; + } + + _position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + + if (_position_setpoint_triplet_sub < 0) { + return -errno; + } + + return 0; +} + +int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) +{ + // already_active is unused, we don't care what happened previously. + + // Default to no change, set if we receive anything. + *control_data = nullptr; + + const int num_poll = 2; + px4_pollfd_struct_t polls[num_poll]; + polls[0].fd = _vehicle_roi_sub; + polls[0].events = POLLIN; + polls[1].fd = _position_setpoint_triplet_sub; + polls[1].events = POLLIN; + + int ret = px4_poll(polls, num_poll, timeout_ms); + + if (ret < 0) { + return -errno; + } + + if (ret == 0) { + // Timeout, _control_data is already null + + } else { + if (polls[0].revents & POLLIN) { + vehicle_roi_s vehicle_roi; + orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); + + _control_data.gimbal_shutter_retract = false; + + if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { + + _control_data.type = ControlData::Type::Neutral; + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { + _control_data.type = ControlData::Type::LonLat; + _read_control_data_from_position_setpoint_sub(); + _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; + + _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; + _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; + _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; + + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { + control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { + //TODO is this even suported? + } + } + + // check whether the position setpoint got updated + if (polls[1].revents & POLLIN) { + if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { + _read_control_data_from_position_setpoint_sub(); + *control_data = &_control_data; + + } else { // must do an orb_copy() in *every* case + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + } + } + } + + return 0; +} + +void InputMavlinkROI::_read_control_data_from_position_setpoint_sub() +{ + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + _control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; + _control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; + _control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; +} + +void InputMavlinkROI::print_status() +{ + PX4_INFO("Input: Mavlink (ROI)"); +} + + +InputMavlinkCmdMount::InputMavlinkCmdMount() +{ +} + +InputMavlinkCmdMount::~InputMavlinkCmdMount() +{ + if (_vehicle_command_sub >= 0) { + orb_unsubscribe(_vehicle_command_sub); + } +} + +int InputMavlinkCmdMount::initialize() +{ + if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) { + return -errno; + } + + // rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode, + // it will publish vehicle_command's as well, causing the input poll() in here to return + // immediately, which in turn will cause an output update and thus a busy loop. + orb_set_interval(_vehicle_command_sub, 10); + + return 0; +} + + +int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) +{ + // Default to notify that there was no change. + *control_data = nullptr; + + const int num_poll = 1; + px4_pollfd_struct_t polls[num_poll]; + polls[0].fd = _vehicle_command_sub; + polls[0].events = POLLIN; + + int poll_timeout = (int)timeout_ms; + + bool exit_loop = false; + + while (!exit_loop && poll_timeout >= 0) { + hrt_abstime poll_start = hrt_absolute_time(); + + int ret = px4_poll(polls, num_poll, poll_timeout); + + if (ret < 0) { + return -errno; + } + + poll_timeout -= (hrt_absolute_time() - poll_start) / 1000; + + // if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout + exit_loop = true; + + if (ret == 0) { + // Timeout control_data already null. + + } else { + if (polls[0].revents & POLLIN) { + vehicle_command_s vehicle_command; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); + + // Process only if the command is for us or for anyone (component id 0). + const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id); + const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) || + (vehicle_command.target_component == 0)); + + if (!sysid_correct || !compid_correct) { + exit_loop = false; + continue; + } + + _control_data.gimbal_shutter_retract = false; + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { + + switch ((int)vehicle_command.param7) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + _control_data.gimbal_shutter_retract = true; + + /* FALLTHROUGH */ + + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + _control_data.type = ControlData::Type::Neutral; + + *control_data = &_control_data; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { + _control_data.type = ControlData::Type::Angle; + _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 + const float roll = math::radians(vehicle_command.param2); + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + const float pitch = math::radians(vehicle_command.param1); + // both specs have yaw on channel 2 + float yaw = math::radians(vehicle_command.param3); + + matrix::Eulerf euler(roll, pitch, yaw); + + matrix::Quatf q(euler); + q.copyTo(_control_data.type_data.angle.q); + + _control_data.type_data.angle.angular_velocity[0] = NAN; + _control_data.type_data.angle.angular_velocity[1] = NAN; + _control_data.type_data.angle.angular_velocity[2] = NAN; + + *control_data = &_control_data; + } + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + control_data_set_lon_lat((double)vehicle_command.param6, (double)vehicle_command.param5, vehicle_command.param4); + + *control_data = &_control_data; + break; + } + + _ack_vehicle_command(&vehicle_command); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + _control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1; + _control_data.stabilize_axis[1] = (int)(vehicle_command.param3 + 0.5f) == 1; + _control_data.stabilize_axis[2] = (int)(vehicle_command.param4 + 0.5f) == 1; + + + const int params[] = { + (int)((float)vehicle_command.param5 + 0.5f), + (int)((float)vehicle_command.param6 + 0.5f), + (int)(vehicle_command.param7 + 0.5f) + }; + + for (int i = 0; i < 3; ++i) { + + if (params[i] == 0) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + } else if (params[i] == 1) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngularRate; + + } else if (params[i] == 2) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + + } else { + // Not supported, fallback to body angle. + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + } + } + + _control_data.type = ControlData::Type::Neutral; //always switch to neutral position + + *control_data = &_control_data; + _ack_vehicle_command(&vehicle_command); + + } else { + exit_loop = false; + } + } + + } + } + + return 0; +} + +void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd) +{ + vehicle_command_ack_s vehicle_command_ack{}; + + vehicle_command_ack.timestamp = hrt_absolute_time(); + vehicle_command_ack.command = cmd->command; + vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + vehicle_command_ack.target_system = cmd->source_system; + vehicle_command_ack.target_component = cmd->source_component; + + uORB::Publication cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); +} + +void InputMavlinkCmdMount::print_status() +{ + PX4_INFO("Input: Mavlink (CMD_MOUNT)"); +} + +InputMavlinkGimbalV2::InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, + float &mnt_rate_pitch, float &mnt_rate_yaw) : + _mav_sys_id(mav_sys_id), + _mav_comp_id(mav_comp_id), + _mnt_rate_pitch(mnt_rate_pitch), + _mnt_rate_yaw(mnt_rate_yaw) +{ + _stream_gimbal_manager_information(); +} + +InputMavlinkGimbalV2::~InputMavlinkGimbalV2() +{ + if (_vehicle_roi_sub >= 0) { + orb_unsubscribe(_vehicle_roi_sub); + } + + if (_position_setpoint_triplet_sub >= 0) { + orb_unsubscribe(_position_setpoint_triplet_sub); + } + + if (_gimbal_manager_set_attitude_sub >= 0) { + orb_unsubscribe(_gimbal_manager_set_attitude_sub); + } + + if (_vehicle_command_sub >= 0) { + orb_unsubscribe(_vehicle_command_sub); + } + + if (_gimbal_manager_set_manual_control_sub >= 0) { + orb_unsubscribe(_gimbal_manager_set_manual_control_sub); + } +} + + +void InputMavlinkGimbalV2::print_status() +{ + PX4_INFO("Input: Mavlink (Gimbal V2)"); +} + +int InputMavlinkGimbalV2::initialize() +{ + _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); + + if (_vehicle_roi_sub < 0) { + return -errno; + } + + _position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + + if (_position_setpoint_triplet_sub < 0) { + return -errno; + } + + _gimbal_manager_set_attitude_sub = orb_subscribe(ORB_ID(gimbal_manager_set_attitude)); + + if (_gimbal_manager_set_attitude_sub < 0) { + return -errno; + } + + if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) { + return -errno; + } + + if ((_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control))) < 0) { + return -errno; + } + + // rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode, + // it will publish vehicle_command's as well, causing the input poll() in here to return + // immediately, which in turn will cause an output update and thus a busy loop. + orb_set_interval(_vehicle_command_sub, 10); + + return 0; +} + +void InputMavlinkGimbalV2::_stream_gimbal_manager_status() +{ + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.updated()) { + _gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status); + } + + gimbal_manager_status_s gimbal_manager_status{}; + gimbal_manager_status.timestamp = hrt_absolute_time(); + gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags; + gimbal_manager_status.gimbal_device_id = 0; + gimbal_manager_status.primary_control_sysid = _sys_id_primary_control; + gimbal_manager_status.primary_control_compid = _comp_id_primary_control; + gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control + gimbal_manager_status.secondary_control_compid = 0; // TODO: support secondary control + _gimbal_manager_status_pub.publish(gimbal_manager_status); +} + +void InputMavlinkGimbalV2::_stream_gimbal_manager_information() +{ + // TODO: Take gimbal_device_information into account. + + gimbal_manager_information_s gimbal_manager_info; + gimbal_manager_info.timestamp = hrt_absolute_time(); + + gimbal_manager_info.cap_flags = + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; + + gimbal_manager_info.pitch_max = M_PI_F / 2; + gimbal_manager_info.pitch_min = -M_PI_F / 2; + gimbal_manager_info.yaw_max = M_PI_F; + gimbal_manager_info.yaw_min = -M_PI_F; + + _gimbal_manager_info_pub.publish(gimbal_manager_info); +} + +int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) +{ + _stream_gimbal_manager_status(); + + // Default to no change, set if we receive anything. + *control_data = nullptr; + + const int num_poll = 5; + px4_pollfd_struct_t polls[num_poll]; + polls[0].fd = _gimbal_manager_set_attitude_sub; + polls[0].events = POLLIN; + polls[1].fd = _vehicle_roi_sub; + polls[1].events = POLLIN; + polls[2].fd = _position_setpoint_triplet_sub; + polls[2].events = POLLIN; + polls[3].fd = _vehicle_command_sub; + polls[3].events = POLLIN; + polls[4].fd = _gimbal_manager_set_manual_control_sub; + polls[4].events = POLLIN; + + int poll_timeout = (int)timeout_ms; + + bool exit_loop = false; + + while (!exit_loop && poll_timeout >= 0) { + hrt_abstime poll_start = hrt_absolute_time(); + + int ret = px4_poll(polls, num_poll, poll_timeout); + + if (ret < 0) { + return -errno; + } + + poll_timeout -= (hrt_absolute_time() - poll_start) / 1000; + + // if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout + exit_loop = true; + + if (ret == 0) { + // Timeout control_data already null. + + } else { + if (polls[0].revents & POLLIN) { + gimbal_manager_set_attitude_s set_attitude; + orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude); + + if (set_attitude.origin_sysid == _sys_id_primary_control && + set_attitude.origin_compid == _comp_id_primary_control) { + const matrix::Quatf q(set_attitude.q); + const matrix::Vector3f angular_velocity(set_attitude.angular_velocity_x, + set_attitude.angular_velocity_y, + set_attitude.angular_velocity_z); + + _set_control_data_from_set_attitude(set_attitude.flags, q, angular_velocity); + *control_data = &_control_data; + + } else { + PX4_DEBUG("Ignoring gimbal_manager_set_attitude from %d/%d", + set_attitude.origin_sysid, set_attitude.origin_compid); + } + } + + if (polls[1].revents & POLLIN) { + vehicle_roi_s vehicle_roi; + orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); + + _control_data.gimbal_shutter_retract = false; + + if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { + + _control_data.type = ControlData::Type::Neutral; + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { + _control_data.type = ControlData::Type::LonLat; + _read_control_data_from_position_setpoint_sub(); + _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; + + _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; + _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; + _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; + + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { + control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { + //TODO is this even suported? + exit_loop = false; + + } else { + exit_loop = false; + } + } + + // check whether the position setpoint got updated + if (polls[2].revents & POLLIN) { + if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { + _read_control_data_from_position_setpoint_sub(); + *control_data = &_control_data; + + } else { // must do an orb_copy() in *every* case + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + exit_loop = false; + } + } + + if (polls[3].revents & POLLIN) { + vehicle_command_s vehicle_command; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); + + // Process only if the command is for us or for anyone (component id 0). + const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id) || (vehicle_command.target_system == 0); + const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) || + (vehicle_command.target_component == 0)); + + if (!sysid_correct || !compid_correct) { + exit_loop = false; + continue; + } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { + // FIXME: Remove me later. For now, we support this for legacy missions + // using gimbal v1 protocol. + + switch ((int)vehicle_command.param7) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + _control_data.gimbal_shutter_retract = true; + + /* FALLTHROUGH */ + + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + _control_data.type = ControlData::Type::Neutral; + + *control_data = &_control_data; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { + _control_data.type = ControlData::Type::Angle; + _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 + const float roll = math::radians(vehicle_command.param2); + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + const float pitch = math::radians(vehicle_command.param1); + // both specs have yaw on channel 2 + float yaw = math::radians(vehicle_command.param3); + + // We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that. + if (yaw > M_PI_F) { + yaw -= 2 * M_PI_F; + } + + matrix::Eulerf euler(roll, pitch, yaw); + + matrix::Quatf q(euler); + q.copyTo(_control_data.type_data.angle.q); + + _control_data.type_data.angle.angular_velocity[0] = NAN; + _control_data.type_data.angle.angular_velocity[1] = NAN; + _control_data.type_data.angle.angular_velocity[2] = NAN; + + *control_data = &_control_data; + } + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + control_data_set_lon_lat((double)vehicle_command.param6, (double)vehicle_command.param5, vehicle_command.param4); + + *control_data = &_control_data; + break; + } + + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + _control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1; + _control_data.stabilize_axis[1] = (int)(vehicle_command.param3 + 0.5f) == 1; + _control_data.stabilize_axis[2] = (int)(vehicle_command.param4 + 0.5f) == 1; + + + const int params[] = { + (int)((float)vehicle_command.param5 + 0.5f), + (int)((float)vehicle_command.param6 + 0.5f), + (int)(vehicle_command.param7 + 0.5f) + }; + + for (int i = 0; i < 3; ++i) { + + if (params[i] == 0) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + } else if (params[i] == 1) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngularRate; + + } else if (params[i] == 2) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + + } else { + // Not supported, fallback to body angle. + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + } + } + + _control_data.type = ControlData::Type::Neutral; //always switch to neutral position + + *control_data = &_control_data; + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { + + const int param_sys_id = roundf(vehicle_command.param1); + const int param_comp_id = roundf(vehicle_command.param2); + + uint8_t new_sys_id_primary_control = [&]() { + if (param_sys_id >= 0 && param_sys_id < 256) { + // Valid new sysid. + return (uint8_t)param_sys_id; + + } else if (param_sys_id == -1) { + // leave unchanged + return _sys_id_primary_control; + + } else if (param_sys_id == -2) { + // set itself + return _mav_sys_id; + + } else if (param_sys_id == -3) { + // release control if in control + if (_sys_id_primary_control == vehicle_command.source_system) { + return (uint8_t)0; + + } else { + return _sys_id_primary_control; + } + + } else { + PX4_WARN("Unknown param1 value for DO_GIMBAL_MANAGER_CONFIGURE"); + return _sys_id_primary_control; + } + }(); + + uint8_t new_comp_id_primary_control = [&]() { + if (param_comp_id >= 0 && param_comp_id < 256) { + // Valid new compid. + return (uint8_t)param_comp_id; + + } else if (param_comp_id == -1) { + // leave unchanged + return _comp_id_primary_control; + + } else if (param_comp_id == -2) { + // set itself + return _mav_comp_id; + + } else if (param_comp_id == -3) { + // release control if in control + if (_comp_id_primary_control == vehicle_command.source_component) { + return (uint8_t)0; + + } else { + return _comp_id_primary_control; + } + + } else { + PX4_WARN("Unknown param2 value for DO_GIMBAL_MANAGER_CONFIGURE"); + return _comp_id_primary_control; + } + }(); + + + if (new_sys_id_primary_control != _sys_id_primary_control || + new_comp_id_primary_control != _comp_id_primary_control) { + PX4_INFO("Configured primary gimbal control sysid/compid from %d/%d to %d/%d", + _sys_id_primary_control, _comp_id_primary_control, + new_sys_id_primary_control, new_comp_id_primary_control); + _sys_id_primary_control = new_sys_id_primary_control; + _comp_id_primary_control = new_comp_id_primary_control; + } + + // TODO: support secondary control + // TODO: support gimbal device id for multiple gimbals + + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { + + if (vehicle_command.source_system == _sys_id_primary_control && + vehicle_command.source_component == _comp_id_primary_control) { + + const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), math::radians(vehicle_command.param2)); + const matrix::Quatf q(euler); + const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, vehicle_command.param4); + const uint32_t flags = vehicle_command.param5; + + // TODO: support gimbal device id for multiple gimbals + + _set_control_data_from_set_attitude(flags, q, angular_velocity); + *control_data = &_control_data; + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + PX4_ERR("GIMBAL_MANAGER_PITCHYAW denied, not in control"); + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + } + + } else { + exit_loop = false; + } + + } + + if (polls[4].revents & POLLIN) { + gimbal_manager_set_manual_control_s set_manual_control; + orb_copy(ORB_ID(gimbal_manager_set_manual_control), _gimbal_manager_set_manual_control_sub, &set_manual_control); + + if (set_manual_control.origin_sysid == _sys_id_primary_control && + set_manual_control.origin_compid == _comp_id_primary_control) { + + const matrix::Quatf q = + (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) ? + matrix::Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) : + matrix::Quatf(NAN, NAN, NAN, NAN); + + const matrix::Vector3f angular_velocity = + (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) ? + matrix::Vector3f(0.0f, + math::radians(_mnt_rate_pitch) * set_manual_control.pitch_rate, + math::radians(_mnt_rate_yaw) * set_manual_control.yaw_rate) : + matrix::Vector3f(NAN, NAN, NAN); + + _set_control_data_from_set_attitude(set_manual_control.flags, q, angular_velocity); + *control_data = &_control_data; + + } else { + PX4_DEBUG("Ignoring gimbal_manager_set_manual_control from %d/%d", + set_manual_control.origin_sysid, set_manual_control.origin_compid); + } + } + } + } + + return 0; +} + +void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, + const matrix::Vector3f &angular_velocity) +{ + if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_RETRACT) != 0) { + // not implemented in ControlData + } else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL) != 0) { + _control_data.type = ControlData::Type::Neutral; + + } else { + _control_data.type = ControlData::Type::Angle; + + q.copyTo(_control_data.type_data.angle.q); + + _control_data.type_data.angle.angular_velocity[0] = angular_velocity(0); + _control_data.type_data.angle.angular_velocity[1] = angular_velocity(1); + _control_data.type_data.angle.angular_velocity[2] = angular_velocity(2); + + _control_data.type_data.angle.frames[0] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + _control_data.type_data.angle.frames[1] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + _control_data.type_data.angle.frames[2] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + } +} + +//TODO move this one to input.cpp such that it can be shared across functions +void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd, uint8_t result) +{ + vehicle_command_ack_s vehicle_command_ack{}; + + vehicle_command_ack.timestamp = hrt_absolute_time(); + vehicle_command_ack.command = cmd->command; + vehicle_command_ack.result = result; + vehicle_command_ack.target_system = cmd->source_system; + vehicle_command_ack.target_component = cmd->source_component; + + uORB::Publication cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); +} + +void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub() +{ + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + _control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; + _control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; + _control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; +} + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/input_mavlink.h b/src/prometheus_px4/src/modules/vmount/input_mavlink.h new file mode 100644 index 0000000..dfe491d --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/input_mavlink.h @@ -0,0 +1,151 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 input_mavlink.h + * @author Beat Küng + * + */ + +#pragma once + +#include "input.h" +#include "input_rc.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace vmount +{ +/** + ** class InputMavlinkROI + ** Input based on the vehicle_roi topic + */ +class InputMavlinkROI : public InputBase +{ +public: + InputMavlinkROI() = default; + virtual ~InputMavlinkROI(); + + virtual void print_status(); + +protected: + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); + virtual int initialize(); + +private: + void _read_control_data_from_position_setpoint_sub(); + + int _vehicle_roi_sub = -1; + int _position_setpoint_triplet_sub = -1; + uint8_t _cur_roi_mode {vehicle_roi_s::ROI_NONE}; +}; + + +/** + ** class InputMavlinkCmdMount + ** Input based on the VEHICLE_CMD_DO_MOUNT_CONTROL mavlink command + */ +class InputMavlinkCmdMount : public InputBase +{ +public: + InputMavlinkCmdMount(); + virtual ~InputMavlinkCmdMount(); + + virtual void print_status(); + +protected: + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); + virtual int initialize(); + +private: + void _ack_vehicle_command(vehicle_command_s *cmd); + + int _vehicle_command_sub = -1; + + int32_t _mav_sys_id{1}; ///< our mavlink system id + int32_t _mav_comp_id{1}; ///< our mavlink component id +}; + +class InputMavlinkGimbalV2 : public InputBase +{ +public: + InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, float &mnt_rate_yaw); + virtual ~InputMavlinkGimbalV2(); + + virtual void print_status(); + +protected: + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); + virtual int initialize(); + +private: + void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, + const matrix::Vector3f &angular_velocity); + void _ack_vehicle_command(vehicle_command_s *cmd, uint8_t result); + void _stream_gimbal_manager_information(); + void _stream_gimbal_manager_status(); + void _read_control_data_from_position_setpoint_sub(); + + int _vehicle_roi_sub = -1; + int _gimbal_manager_set_attitude_sub = -1; + int _gimbal_manager_set_manual_control_sub = -1; + int _position_setpoint_triplet_sub = -1; + int _vehicle_command_sub = -1; + + uint8_t _mav_sys_id{1}; ///< our mavlink system id + uint8_t _mav_comp_id{1}; ///< our mavlink component id + + uint8_t _sys_id_primary_control{0}; + uint8_t _comp_id_primary_control{0}; + + float &_mnt_rate_pitch; + float &_mnt_rate_yaw; + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Publication _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)}; + uORB::Publication _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; + uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; +}; + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/input_rc.cpp b/src/prometheus_px4/src/modules/vmount/input_rc.cpp new file mode 100644 index 0000000..de5a5d8 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/input_rc.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 input_rc.cpp + * @author Beat Küng + * + */ + +#include "input_rc.h" + +#include +#include +#include +#include +#include + + +namespace vmount +{ + + +InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw) +{ + _aux_channels[0] = aux_channel_roll; + _aux_channels[1] = aux_channel_pitch; + _aux_channels[2] = aux_channel_yaw; +} + +InputRC::~InputRC() +{ + if (_manual_control_setpoint_sub >= 0) { + orb_unsubscribe(_manual_control_setpoint_sub); + } +} + +int InputRC::initialize() +{ + _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + if (_manual_control_setpoint_sub < 0) { + return -errno; + } + + return 0; +} + +int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) +{ + // Default to no change signalled by NULL. + *control_data = nullptr; + + px4_pollfd_struct_t polls[1]; + polls[0].fd = _manual_control_setpoint_sub; + polls[0].events = POLLIN; + + int ret = px4_poll(polls, 1, timeout_ms); + + if (ret < 0) { + return -errno; + } + + if (ret == 0) { + // Timeout, leave NULL + } else { + if (polls[0].revents & POLLIN) { + // Only if there was a change, we update the control data, otherwise leave it NULL. + if (_read_control_data_from_subscription(_control_data, already_active)) { + *control_data = &_control_data; + } + } + } + + return 0; +} + +bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active) +{ + manual_control_setpoint_s manual_control_setpoint; + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint); + control_data.type = ControlData::Type::Angle; + + float new_aux_values[3]; + + for (int i = 0; i < 3; ++i) { + new_aux_values[i] = _get_aux_value(manual_control_setpoint, i); + } + + // If we were already active previously, we just update normally. Otherwise, there needs to be + // a major stick movement to re-activate manual (or it's running for the very first time). + bool major_movement = false; + + // Detect a big stick movement + for (int i = 0; i < 3; ++i) { + if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) { + major_movement = true; + } + } + + if (already_active || major_movement || _first_time) { + + _first_time = false; + + matrix::Eulerf euler(new_aux_values[0] * M_PI_F, new_aux_values[1] * M_PI_F, + new_aux_values[2] * M_PI_F); + matrix::Quatf q(euler); + q.copyTo(control_data.type_data.angle.q); + + for (int i = 0; i < 3; ++i) { + // We always use follow mode with RC input for now. + control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _last_set_aux_values[i] = new_aux_values[i]; + } + + control_data.gimbal_shutter_retract = false; + return true; + + } else { + return false; + } +} + +float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx) +{ + switch (_aux_channels[channel_idx]) { + + case 1: + return manual_control_setpoint.aux1; + + case 2: + return manual_control_setpoint.aux2; + + case 3: + return manual_control_setpoint.aux3; + + case 4: + return manual_control_setpoint.aux4; + + case 5: + return manual_control_setpoint.aux5; + + case 6: + return manual_control_setpoint.aux6; + + default: + return 0.0f; + } +} + +void InputRC::print_status() +{ + PX4_INFO("Input: RC (channels: roll=%i, pitch=%i, yaw=%i)", _aux_channels[0], _aux_channels[1], _aux_channels[2]); +} + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/input_rc.h b/src/prometheus_px4/src/modules/vmount/input_rc.h new file mode 100644 index 0000000..89cf3a6 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/input_rc.h @@ -0,0 +1,91 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 input_rc.h + * @author Beat Küng + * + */ + +#pragma once + +#include "input.h" +#include + +namespace vmount +{ + +class InputMavlinkROI; +class InputMavlinkCmdMount; + +/** + ** class InputRC + * RC input class using manual_control_setpoint topic + */ +class InputRC : public InputBase +{ +public: + + /** + * @param aux_channel_roll which aux channel to use for roll (set to 0 to use a fixed angle of 0) + * @param aux_channel_pitch + * @param aux_channel_yaw + */ + InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw); + virtual ~InputRC(); + + virtual void print_status(); + +protected: + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); + virtual int initialize(); + + /** + * @return true if there was a change in control data + */ + virtual bool _read_control_data_from_subscription(ControlData &control_data, bool already_active); + + int _get_subscription_fd() const { return _manual_control_setpoint_sub; } + + float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx); + +private: + int _aux_channels[3]; + int _manual_control_setpoint_sub = -1; + + bool _first_time = true; + float _last_set_aux_values[3] = {}; +}; + + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/input_test.cpp b/src/prometheus_px4/src/modules/vmount/input_test.cpp new file mode 100644 index 0000000..4670521 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/input_test.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 input_test.cpp + * @author Beat Küng + * + */ + +#include "input_test.h" + +#include + +#include +#include +#include + + +namespace vmount +{ + +InputTest::InputTest(float roll_deg, float pitch_deg, float yaw_deg) +{ + _angles[0] = roll_deg; + _angles[1] = pitch_deg; + _angles[2] = yaw_deg; +} + +bool InputTest::finished() +{ + return true; /* only a single-shot test (for now) */ +} + +int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool already_active) +{ + //we directly override the update() here, since we don't need the initialization from the base class + + _control_data.type = ControlData::Type::Angle; + + _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + matrix::Eulerf euler( + math::radians(_angles[0]), + math::radians(_angles[1]), + math::radians(_angles[2])); + matrix::Quatf q(euler); + + q.copyTo(_control_data.type_data.angle.q); + + _control_data.gimbal_shutter_retract = false; + *control_data = &_control_data; + return 0; +} + +int InputTest::initialize() +{ + return 0; +} + +void InputTest::print_status() +{ + PX4_INFO("Input: Test"); +} + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/input_test.h b/src/prometheus_px4/src/modules/vmount/input_test.h new file mode 100644 index 0000000..15e3add --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/input_test.h @@ -0,0 +1,78 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 input_test.h + * @author Beat Küng + * + */ + +#pragma once + +#include "input.h" + +namespace vmount +{ + +/** + ** class InputTest + * Send a single control command, configured via constructor arguments + */ +class InputTest : public InputBase +{ +public: + + /** + * set to a fixed angle + */ + InputTest(float roll_deg, float pitch_deg, float yaw_deg); + virtual ~InputTest() {} + + /** check whether the test finished, and thus the main thread can quit */ + bool finished(); + + virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active); + +protected: + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { return 0; } //not needed + + virtual int initialize(); + + virtual void print_status(); + +private: + float _angles[3]; /**< desired angles in [deg] */ +}; + + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/output.cpp b/src/prometheus_px4/src/modules/vmount/output.cpp new file mode 100644 index 0000000..913274a --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/output.cpp @@ -0,0 +1,241 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 output.cpp + * @author Leon Müller (thedevleon) + * @author Beat Küng + * + */ + +#include "output.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vmount +{ + +OutputBase::OutputBase(const OutputConfig &output_config) + : _config(output_config) +{ + _last_update = hrt_absolute_time(); +} + +void OutputBase::publish() +{ + mount_orientation_s mount_orientation{}; + + for (unsigned i = 0; i < 3; ++i) { + mount_orientation.attitude_euler_angle[i] = _angle_outputs[i]; + } + + mount_orientation.timestamp = hrt_absolute_time(); + _mount_orientation_pub.publish(mount_orientation); +} + +float OutputBase::_calculate_pitch(double lon, double lat, float altitude, + const vehicle_global_position_s &global_position) +{ + if (!map_projection_initialized(&_projection_reference)) { + map_projection_init(&_projection_reference, global_position.lat, global_position.lon); + } + + float x1, y1, x2, y2; + map_projection_project(&_projection_reference, lat, lon, &x1, &y1); + map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2); + float dx = x1 - x2, dy = y1 - y2; + float target_distance = sqrtf(dx * dx + dy * dy); + float z = altitude - global_position.alt; + + return atan2f(z, target_distance); +} + +void OutputBase::_set_angle_setpoints(const ControlData *control_data) +{ + _cur_control_data = control_data; + + switch (control_data->type) { + case ControlData::Type::Angle: + + { + for (int i = 0; i < 3; ++i) { + switch (control_data->type_data.angle.frames[i]) { + case ControlData::TypeData::TypeAngle::Frame::AngularRate: + break; + + case ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame: + _absolute_angle[i] = false; + break; + + case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame: + _absolute_angle[i] = true; + break; + } + + _angle_velocity[i] = control_data->type_data.angle.angular_velocity[i]; + } + + for (int i = 0; i < 4; ++i) { + _q_setpoint[i] = control_data->type_data.angle.q[i]; + } + } + + break; + + case ControlData::Type::LonLat: + _handle_position_update(true); + break; + + case ControlData::Type::Neutral: + _q_setpoint[0] = 1.f; + _q_setpoint[1] = 0.f; + _q_setpoint[2] = 0.f; + _q_setpoint[3] = 0.f; + _angle_velocity[0] = NAN; + _angle_velocity[1] = NAN; + _angle_velocity[2] = NAN; + break; + } + + for (int i = 0; i < 3; ++i) { + _stabilize[i] = control_data->stabilize_axis[i]; + } +} + +void OutputBase::_handle_position_update(bool force_update) +{ + if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) { + return; + } + + vehicle_global_position_s vehicle_global_position{}; + vehicle_local_position_s vehicle_local_position{}; + + if (force_update) { + _vehicle_global_position_sub.copy(&vehicle_global_position); + _vehicle_local_position_sub.copy(&vehicle_local_position); + + } else { + if (!_vehicle_global_position_sub.update(&vehicle_global_position)) { + return; + } + + if (!_vehicle_local_position_sub.update(&vehicle_local_position)) { + return; + } + } + + const double &vlat = vehicle_global_position.lat; + const double &vlon = vehicle_global_position.lon; + + const double &lat = _cur_control_data->type_data.lonlat.lat; + const double &lon = _cur_control_data->type_data.lonlat.lon; + const float &alt = _cur_control_data->type_data.lonlat.altitude; + + float roll = _cur_control_data->type_data.lonlat.roll_angle; + + // interface: use fixed pitch value > -pi otherwise consider ROI altitude + float pitch = (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ? + _cur_control_data->type_data.lonlat.pitch_fixed_angle : + _calculate_pitch(lon, lat, alt, vehicle_global_position); + + float yaw = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading; + + // add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET + pitch += _cur_control_data->type_data.lonlat.pitch_angle_offset; + yaw += _cur_control_data->type_data.lonlat.yaw_angle_offset; + + matrix::Quatf(matrix::Eulerf(roll, pitch, yaw)).copyTo(_q_setpoint); + + _angle_velocity[0] = NAN; + _angle_velocity[1] = NAN; + _angle_velocity[2] = NAN; +} + +void OutputBase::_calculate_angle_output(const hrt_abstime &t) +{ + //get the output angles and stabilize if necessary + vehicle_attitude_s vehicle_attitude{}; + matrix::Eulerf euler_vehicle; + + // We only need to apply additional compensation if the required angle is + // absolute (world frame) as well as the gimbal is not capable of doing that + // calculation. (Most gimbals stabilize at least roll and pitch + // and only need compensation for yaw, if at all.) + bool compensate[3]; + + for (int i = 0; i < 3; ++i) { + compensate[i] = _stabilize[i] && _absolute_angle[i]; + } + + if (compensate[0] || compensate[1] || compensate[2]) { + _vehicle_attitude_sub.copy(&vehicle_attitude); + euler_vehicle = matrix::Quatf(vehicle_attitude.q); + } + + float dt = (t - _last_update) / 1.e6f; + + matrix::Eulerf euler_gimbal = matrix::Quatf(_q_setpoint); + + for (int i = 0; i < 3; ++i) { + + if (PX4_ISFINITE(euler_gimbal(i))) { + _angle_outputs[i] = euler_gimbal(i); + } + + if (PX4_ISFINITE(_angle_velocity[i])) { + _angle_outputs[i] += dt * _angle_velocity[i]; + } + + if (compensate[i]) { + _angle_outputs[i] -= euler_vehicle(i); + } + + if (PX4_ISFINITE(_angle_outputs[i])) { + //bring angles into proper range [-pi, pi] + _angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]); + } + } +} + +} /* namespace vmount */ + diff --git a/src/prometheus_px4/src/modules/vmount/output.h b/src/prometheus_px4/src/modules/vmount/output.h new file mode 100644 index 0000000..21e6f09 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/output.h @@ -0,0 +1,140 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 output.h + * @author Beat Küng + * + */ + +#pragma once + +#include "common.h" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vmount +{ + +struct OutputConfig { + float gimbal_retracted_mode_value; /**< Mixer output value for selecting gimbal retracted mode */ + float gimbal_normal_mode_value; /**< Mixer output value for selecting gimbal normal mode */ + + /** Scale factor for pitch channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */ + float pitch_scale; + /** Scale factor for roll channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */ + float roll_scale; + /** Scale factor for yaw channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */ + float yaw_scale; + + float pitch_offset; /**< Offset for pitch channel in radians */ + float roll_offset; /**< Offset for roll channel in radians */ + float yaw_offset; /**< Offset for yaw channel in radians */ + + uint32_t mavlink_sys_id_v1; /**< Mavlink target system id for mavlink output only for v1 */ + uint32_t mavlink_comp_id_v1; +}; + + +/** + ** class OutputBase + * Base class for all driver output classes + */ +class OutputBase +{ +public: + OutputBase(const OutputConfig &output_config); + virtual ~OutputBase() = default; + + virtual int initialize() { return 0; } + + /** + * Update the output. + * @param data new command if non null + * @return 0 on success, <0 otherwise + */ + virtual int update(const ControlData *control_data) = 0; + + /** report status to stdout */ + virtual void print_status() = 0; + + /** Publish _angle_outputs as a mount_orientation message. */ + void publish(); + +protected: + float _calculate_pitch(double lon, double lat, float altitude, + const vehicle_global_position_s &global_position); + + map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] + + const OutputConfig &_config; + + /** set angle setpoints, speeds & stabilize flags */ + void _set_angle_setpoints(const ControlData *control_data); + + /** check if vehicle position changed and update the setpoint angles if in gps mode */ + void _handle_position_update(bool force_update = false); + + const ControlData *_cur_control_data = nullptr; + + float _q_setpoint[4] = { NAN, NAN, NAN, NAN }; ///< can be NAN if not specifically set + float _angle_velocity[3] = { NAN, NAN, NAN }; //< [rad/s], can be NAN if not specifically set + + bool _stabilize[3] = { false, false, false }; + + // Pitch and role are by default aligned with the horizon. + // Yaw follows the vehicle (not lock/absolute mode). + bool _absolute_angle[3] = {true, true, false }; + + /** calculate the _angle_outputs (with speed) and stabilize if needed */ + void _calculate_angle_output(const hrt_abstime &t); + + float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad] + hrt_abstime _last_update; + +private: + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + uORB::Publication _mount_orientation_pub{ORB_ID(mount_orientation)}; +}; + + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/output_mavlink.cpp b/src/prometheus_px4/src/modules/vmount/output_mavlink.cpp new file mode 100644 index 0000000..c8ea655 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/output_mavlink.cpp @@ -0,0 +1,214 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 output_mavlink.cpp + * @author Leon Müller (thedevleon) + * @author Beat Küng + * + */ + +#include "output_mavlink.h" + +#include +#include + +#include +#include + + +namespace vmount +{ + +OutputMavlinkV1::OutputMavlinkV1(const OutputConfig &output_config) + : OutputBase(output_config) +{ +} + +int OutputMavlinkV1::update(const ControlData *control_data) +{ + vehicle_command_s vehicle_command{}; + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id_v1; + vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id_v1; + + if (control_data) { + //got new command + _set_angle_setpoints(control_data); + + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE; + vehicle_command.timestamp = hrt_absolute_time(); + + if (control_data->type == ControlData::Type::Neutral) { + vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; + + vehicle_command.param5 = 0.0; + vehicle_command.param6 = 0.0; + vehicle_command.param7 = 0.0f; + + } else { + vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; + + vehicle_command.param5 = static_cast(control_data->type_data.angle.frames[0]); + vehicle_command.param6 = static_cast(control_data->type_data.angle.frames[1]); + vehicle_command.param7 = static_cast(control_data->type_data.angle.frames[2]); + } + + vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f; + vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f; + vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f; + + _vehicle_command_pub.publish(vehicle_command); + } + + _handle_position_update(); + + hrt_abstime t = hrt_absolute_time(); + _calculate_angle_output(t); + + vehicle_command.timestamp = t; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL; + + // vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively + // vmount uses radians, MAVLink uses degrees + vehicle_command.param1 = math::degrees(_angle_outputs[1] + _config.pitch_offset); + vehicle_command.param2 = math::degrees(_angle_outputs[0] + _config.roll_offset); + vehicle_command.param3 = math::degrees(_angle_outputs[2] + _config.yaw_offset); + vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING; + + _vehicle_command_pub.publish(vehicle_command); + + _last_update = t; + + return 0; +} + +void OutputMavlinkV1::print_status() +{ + PX4_INFO("Output: MAVLink gimbal protocol v1"); +} + +OutputMavlinkV2::OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config) + : OutputBase(output_config), + _mav_sys_id(mav_sys_id), + _mav_comp_id(mav_comp_id) +{ +} + +int OutputMavlinkV2::update(const ControlData *control_data) +{ + _check_for_gimbal_device_information(); + + hrt_abstime t = hrt_absolute_time(); + + if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) { + _request_gimbal_device_information(); + _last_gimbal_device_checked = t; + + } else { + if (control_data) { + //got new command + _set_angle_setpoints(control_data); + } + + _handle_position_update(); + _publish_gimbal_device_set_attitude(); + _last_update = t; + } + + return 0; +} + +void OutputMavlinkV2::_request_gimbal_device_information() +{ + vehicle_command_s vehicle_cmd{}; + vehicle_cmd.timestamp = hrt_absolute_time(); + vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE; + vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION; + vehicle_cmd.target_system = 0; + vehicle_cmd.target_component = 0; + vehicle_cmd.source_system = _mav_sys_id; + vehicle_cmd.source_component = _mav_comp_id; + vehicle_cmd.confirmation = 0; + vehicle_cmd.from_external = false; + + uORB::Publication vehicle_command_pub{ORB_ID(vehicle_command)}; + vehicle_command_pub.publish(vehicle_cmd); +} + +void OutputMavlinkV2::_check_for_gimbal_device_information() +{ + gimbal_device_information_s gimbal_device_information; + + if (_gimbal_device_information_sub.update(&gimbal_device_information)) { + _gimbal_device_found = true; + _gimbal_device_compid = gimbal_device_information.gimbal_device_compid; + } +} + +void OutputMavlinkV2::print_status() +{ + PX4_INFO("Output: MAVLink gimbal protocol v2"); +} + +void OutputMavlinkV2::_publish_gimbal_device_set_attitude() +{ + gimbal_device_set_attitude_s set_attitude{}; + set_attitude.timestamp = hrt_absolute_time(); + set_attitude.target_system = (uint8_t)_mav_sys_id; + set_attitude.target_component = _gimbal_device_compid; + + set_attitude.angular_velocity_x = _angle_velocity[0]; + set_attitude.angular_velocity_y = _angle_velocity[1]; + set_attitude.angular_velocity_z = _angle_velocity[2]; + set_attitude.q[0] = _q_setpoint[0]; + set_attitude.q[1] = _q_setpoint[1]; + set_attitude.q[2] = _q_setpoint[2]; + set_attitude.q[3] = _q_setpoint[3]; + + if (_absolute_angle[0]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK; + } + + if (_absolute_angle[1]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + } + + if (_absolute_angle[2]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK; + } + + _gimbal_device_set_attitude_pub.publish(set_attitude); +} + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/output_mavlink.h b/src/prometheus_px4/src/modules/vmount/output_mavlink.h new file mode 100644 index 0000000..89eb4a6 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/output_mavlink.h @@ -0,0 +1,95 @@ +/**************************************************************************** +* +* Copyright (c) 2016-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 output_mavlink.h + * @author Beat Küng + * + */ + +#pragma once + +#include "output.h" + +#include +#include +#include +#include + + +namespace vmount +{ +/** + ** class OutputMavlink + * Output via vehicle_command topic + */ +class OutputMavlinkV1 : public OutputBase +{ +public: + OutputMavlinkV1(const OutputConfig &output_config); + virtual ~OutputMavlinkV1() = default; + + virtual int update(const ControlData *control_data); + + virtual void print_status(); + +private: + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; +}; + +class OutputMavlinkV2 : public OutputBase +{ +public: + OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config); + virtual ~OutputMavlinkV2() = default; + + virtual int update(const ControlData *control_data); + + virtual void print_status(); + +private: + void _publish_gimbal_device_set_attitude(); + void _request_gimbal_device_information(); + void _check_for_gimbal_device_information(); + + uORB::Publication _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)}; + uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; + + int32_t _mav_sys_id{1}; ///< our mavlink system id + int32_t _mav_comp_id{1}; ///< our mavlink component id + uint8_t _gimbal_device_compid{0}; + hrt_abstime _last_gimbal_device_checked{0}; + bool _gimbal_device_found {false}; +}; + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/output_rc.cpp b/src/prometheus_px4/src/modules/vmount/output_rc.cpp new file mode 100644 index 0000000..04d2df3 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/output_rc.cpp @@ -0,0 +1,109 @@ +/**************************************************************************** +* +* Copyright (c) 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 output_rc.cpp + * @author Leon Müller (thedevleon) + * @author Beat Küng + * + */ + +#include "output_rc.h" + +#include +#include +#include + +namespace vmount +{ + +OutputRC::OutputRC(const OutputConfig &output_config) + : OutputBase(output_config) +{ +} + +int OutputRC::update(const ControlData *control_data) +{ + if (control_data) { + //got new command + _retract_gimbal = control_data->gimbal_shutter_retract; + _set_angle_setpoints(control_data); + } + + _handle_position_update(); + + hrt_abstime t = hrt_absolute_time(); + _calculate_angle_output(t); + + actuator_controls_s actuator_controls{}; + actuator_controls.timestamp = hrt_absolute_time(); + // _angle_outputs are in radians, actuator_controls are in [-1, 1] + actuator_controls.control[0] = (_angle_outputs[0] + _config.roll_offset) * _config.roll_scale; + actuator_controls.control[1] = (_angle_outputs[1] + _config.pitch_offset) * _config.pitch_scale; + actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale; + actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value; + + _stream_device_attitude_status(); + + _actuator_controls_pub.publish(actuator_controls); + + _last_update = t; + + return 0; +} + +void OutputRC::print_status() +{ + PX4_INFO("Output: AUX"); +} + +void OutputRC::_stream_device_attitude_status() +{ + gimbal_device_attitude_status_s attitude_status{}; + attitude_status.timestamp = hrt_absolute_time(); + attitude_status.target_system = 0; + attitude_status.target_component = 0; + attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | + gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | + gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK | + gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK; + + matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]); + matrix::Quatf q(euler); + q.copyTo(attitude_status.q); + + attitude_status.failure_flags = 0; + _attitude_status_pub.publish(attitude_status); +} + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/output_rc.h b/src/prometheus_px4/src/modules/vmount/output_rc.h new file mode 100644 index 0000000..52839ca --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/output_rc.h @@ -0,0 +1,76 @@ +/**************************************************************************** +* +* Copyright (c) 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 output_rc.h + * @author Beat Küng + * + */ + +#pragma once + +#include "output.h" + +#include +#include +#include + +namespace vmount +{ + + +/** + ** class OutputRC + * Output via actuator_controls_2 topic + */ +class OutputRC : public OutputBase +{ +public: + OutputRC(const OutputConfig &output_config); + virtual ~OutputRC() = default; + + virtual int update(const ControlData *control_data); + + virtual void print_status(); + +private: + void _stream_device_attitude_status(); + + uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_2)}; + uORB::Publication _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; + + bool _retract_gimbal = true; +}; + + +} /* namespace vmount */ diff --git a/src/prometheus_px4/src/modules/vmount/vmount.cpp b/src/prometheus_px4/src/modules/vmount/vmount.cpp new file mode 100644 index 0000000..c6c5fcc --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/vmount.cpp @@ -0,0 +1,668 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020, 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 vmount.cpp + * @author Leon Müller (thedevleon) + * @author Beat Küng + * @author Julian Oes + * @author Matthew Edwards (mje-nz) + * + * Driver for to control mounts such as gimbals or servos. + * Inputs for the mounts can RC and/or mavlink commands. + * Outputs to the mounts can be RC (PWM) output or mavlink. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "input_mavlink.h" +#include "input_rc.h" +#include "input_test.h" +#include "output_rc.h" +#include "output_mavlink.h" + +#include +#include +#include + +#include +#include + +using namespace time_literals; +using namespace vmount; + +/* thread state */ +static volatile bool thread_should_exit = false; +static volatile bool thread_running = false; + +static constexpr int input_objs_len_max = 3; + +struct ThreadData { + InputBase *input_objs[input_objs_len_max] = {nullptr, nullptr, nullptr}; + int input_objs_len = 0; + OutputBase *output_obj = nullptr; +}; +static volatile ThreadData *g_thread_data = nullptr; + +struct Parameters { + int32_t mnt_mode_in; + int32_t mnt_mode_out; + int32_t mnt_mav_sys_id_v1; + int32_t mnt_mav_comp_id_v1; + float mnt_ob_lock_mode; + float mnt_ob_norm_mode; + int32_t mnt_man_pitch; + int32_t mnt_man_roll; + int32_t mnt_man_yaw; + int32_t mnt_do_stab; + float mnt_range_pitch; + float mnt_range_roll; + float mnt_range_yaw; + float mnt_off_pitch; + float mnt_off_roll; + float mnt_off_yaw; + int32_t mav_sys_id; + int32_t mav_comp_id; + float mnt_rate_pitch; + float mnt_rate_yaw; + + bool operator!=(const Parameters &p) + { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" + return mnt_mode_in != p.mnt_mode_in || + mnt_mode_out != p.mnt_mode_out || + mnt_mav_sys_id_v1 != p.mnt_mav_sys_id_v1 || + mnt_mav_comp_id_v1 != p.mnt_mav_comp_id_v1 || + fabsf(mnt_ob_lock_mode - p.mnt_ob_lock_mode) > 1e-6f || + fabsf(mnt_ob_norm_mode - p.mnt_ob_norm_mode) > 1e-6f || + mnt_man_pitch != p.mnt_man_pitch || + mnt_man_roll != p.mnt_man_roll || + mnt_man_yaw != p.mnt_man_yaw || + mnt_do_stab != p.mnt_do_stab || + mnt_range_pitch != p.mnt_range_pitch || + mnt_range_roll != p.mnt_range_roll || + mnt_range_yaw != p.mnt_range_yaw || + mnt_off_pitch != p.mnt_off_pitch || + mnt_off_roll != p.mnt_off_roll || + mnt_off_yaw != p.mnt_off_yaw || + mav_sys_id != p.mav_sys_id || + mav_comp_id != p.mav_comp_id; +#pragma GCC diagnostic pop + + } +}; + +struct ParameterHandles { + param_t mnt_mode_in; + param_t mnt_mode_out; + param_t mnt_mav_sys_id_v1; + param_t mnt_mav_comp_id_v1; + param_t mnt_ob_lock_mode; + param_t mnt_ob_norm_mode; + param_t mnt_man_pitch; + param_t mnt_man_roll; + param_t mnt_man_yaw; + param_t mnt_do_stab; + param_t mnt_range_pitch; + param_t mnt_range_roll; + param_t mnt_range_yaw; + param_t mnt_off_pitch; + param_t mnt_off_roll; + param_t mnt_off_yaw; + param_t mav_sys_id; + param_t mav_comp_id; + param_t mnt_rate_pitch; + param_t mnt_rate_yaw; +}; + + +/* functions */ +static void usage(); +static void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &got_changes); +static bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms); + +static int vmount_thread_main(int argc, char *argv[]); +extern "C" __EXPORT int vmount_main(int argc, char *argv[]); + +static int vmount_thread_main(int argc, char *argv[]) +{ + ParameterHandles param_handles; + Parameters params {}; + OutputConfig output_config; + ThreadData thread_data; + + InputTest *test_input = nullptr; + + // We don't need the task name. + argc -= 1; + argv += 1; + + if (argc > 0 && !strcmp(argv[0], "test")) { + PX4_INFO("Starting in test mode"); + + const char *axis_names[3] = {"roll", "pitch", "yaw"}; + float angles[3] = { 0.f, 0.f, 0.f }; + + if (argc == 3) { + bool found_axis = false; + + for (int i = 0 ; i < 3; ++i) { + if (!strcmp(argv[1], axis_names[i])) { + long angle_deg = strtol(argv[2], nullptr, 0); + angles[i] = (float)angle_deg; + found_axis = true; + } + } + + if (!found_axis) { + usage(); + return -1; + } + + test_input = new InputTest(angles[0], angles[1], angles[2]); + + if (!test_input) { + PX4_ERR("memory allocation failed"); + return -1; + } + + } else { + usage(); + return -1; + } + } + + if (!get_params(param_handles, params)) { + PX4_ERR("could not get mount parameters!"); + delete test_input; + return -1; + } + + uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s}; + thread_running = true; + ControlData *control_data = nullptr; + g_thread_data = &thread_data; + + int last_active = -1; + + while (!thread_should_exit) { + + if (!thread_data.input_objs[0] && (params.mnt_mode_in >= 0 || test_input)) { //need to initialize + + output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode; + output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode; + output_config.pitch_scale = 1.0f / (math::radians(params.mnt_range_pitch / 2.0f)); + output_config.roll_scale = 1.0f / (math::radians(params.mnt_range_roll / 2.0f)); + output_config.yaw_scale = 1.0f / (math::radians(params.mnt_range_yaw / 2.0f)); + output_config.pitch_offset = math::radians(params.mnt_off_pitch); + output_config.roll_offset = math::radians(params.mnt_off_roll); + output_config.yaw_offset = math::radians(params.mnt_off_yaw); + output_config.mavlink_sys_id_v1 = params.mnt_mav_sys_id_v1; + output_config.mavlink_comp_id_v1 = params.mnt_mav_comp_id_v1; + + bool alloc_failed = false; + thread_data.input_objs_len = 1; + + if (test_input) { + thread_data.input_objs[0] = test_input; + + } else { + switch (params.mnt_mode_in) { + case 0: + + // Automatic + thread_data.input_objs[0] = new InputMavlinkCmdMount(); + thread_data.input_objs[1] = new InputMavlinkROI(); + + // RC is on purpose last here so that if there are any mavlink + // messages, they will take precedence over RC. + // This logic is done further below while update() is called. + thread_data.input_objs[2] = new InputRC(params.mnt_man_roll, + params.mnt_man_pitch, + params.mnt_man_yaw); + thread_data.input_objs_len = 3; + + break; + + case 1: //RC + thread_data.input_objs[0] = new InputRC(params.mnt_man_roll, + params.mnt_man_pitch, + params.mnt_man_yaw); + break; + + case 2: //MAVLINK_ROI + thread_data.input_objs[0] = new InputMavlinkROI(); + break; + + case 3: //MAVLINK_DO_MOUNT + thread_data.input_objs[0] = new InputMavlinkCmdMount(); + break; + + case 4: //MAVLINK_V2 + thread_data.input_objs[0] = new InputMavlinkGimbalV2( + params.mav_sys_id, + params.mav_comp_id, + params.mnt_rate_pitch, + params.mnt_rate_yaw); + break; + + default: + PX4_ERR("invalid input mode %" PRId32, params.mnt_mode_in); + break; + } + } + + for (int i = 0; i < thread_data.input_objs_len; ++i) { + if (!thread_data.input_objs[i]) { + alloc_failed = true; + } + } + + switch (params.mnt_mode_out) { + case 0: //AUX + thread_data.output_obj = new OutputRC(output_config); + + if (!thread_data.output_obj) { alloc_failed = true; } + + break; + + case 1: //MAVLink v1 gimbal protocol + thread_data.output_obj = new OutputMavlinkV1(output_config); + + if (!thread_data.output_obj) { alloc_failed = true; } + + break; + + case 2: //MAVLink v2 gimbal protocol + thread_data.output_obj = new OutputMavlinkV2(params.mav_sys_id, params.mav_comp_id, output_config); + + if (!thread_data.output_obj) { alloc_failed = true; } + + break; + + default: + PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out); + thread_should_exit = true; + break; + } + + if (alloc_failed) { + thread_data.input_objs_len = 0; + PX4_ERR("memory allocation failed"); + thread_should_exit = true; + } + + if (thread_should_exit) { + break; + } + + int ret = thread_data.output_obj->initialize(); + + if (ret) { + PX4_ERR("failed to initialize output mode (%i)", ret); + thread_should_exit = true; + break; + } + } + + if (thread_data.input_objs_len > 0) { + + //get input: we cannot make the timeout too large, because the output needs to update + //periodically for stabilization and angle updates. + + for (int i = 0; i < thread_data.input_objs_len; ++i) { + + if (params.mnt_do_stab == 1) { + thread_data.input_objs[i]->set_stabilize(true, true, true); + + } else if (params.mnt_do_stab == 2) { + thread_data.input_objs[i]->set_stabilize(false, false, true); + + } else { + thread_data.input_objs[i]->set_stabilize(false, false, false); + } + + + bool already_active = (last_active == i); + + ControlData *control_data_to_check = nullptr; + unsigned int poll_timeout = already_active ? 50 : 0; // poll only on active input to reduce latency + int ret = thread_data.input_objs[i]->update(poll_timeout, &control_data_to_check, already_active); + + if (ret) { + PX4_ERR("failed to read input %i (ret: %i)", i, ret); + continue; + } + + if (control_data_to_check != nullptr || already_active) { + control_data = control_data_to_check; + last_active = i; + } + } + + //update output + int ret = thread_data.output_obj->update(control_data); + + if (ret) { + PX4_ERR("failed to write output (%i)", ret); + break; + } + + // Only publish the mount orientation if the mode is not mavlink v1 or v2 + // If the gimbal speaks mavlink it publishes its own orientation. + if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2 + thread_data.output_obj->publish(); + } + + + } else { + //wait for parameter changes. We still need to wake up regularily to check for thread exit requests + px4_usleep(1e6); + } + + if (test_input && test_input->finished()) { + thread_should_exit = true; + break; + } + + // check for parameter updates + if (parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + parameter_update_sub.copy(&pupdate); + + // update parameters from storage + bool updated = false; + update_params(param_handles, params, updated); + + if (updated) { + //re-init objects + for (int i = 0; i < input_objs_len_max; ++i) { + if (thread_data.input_objs[i]) { + delete (thread_data.input_objs[i]); + thread_data.input_objs[i] = nullptr; + } + } + + thread_data.input_objs_len = 0; + + last_active = -1; + + if (thread_data.output_obj) { + delete (thread_data.output_obj); + thread_data.output_obj = nullptr; + } + } + } + } + + g_thread_data = nullptr; + + for (int i = 0; i < input_objs_len_max; ++i) { + if (thread_data.input_objs[i]) { + delete (thread_data.input_objs[i]); + thread_data.input_objs[i] = nullptr; + } + } + + thread_data.input_objs_len = 0; + + if (thread_data.output_obj) { + delete (thread_data.output_obj); + thread_data.output_obj = nullptr; + } + + thread_running = false; + return 0; +} + +/** + * The main command function. + * Processes command line arguments and starts the daemon. + */ +int vmount_main(int argc, char *argv[]) +{ + if (argc < 2) { + PX4_ERR("missing command"); + usage(); + return -1; + } + + const bool found_start = !strcmp(argv[1], "start"); + const bool found_test = !strcmp(argv[1], "test"); + + if (found_start || found_test) { + + /* this is not an error */ + if (thread_running) { + if (found_start) { + PX4_WARN("mount driver already running"); + return 0; + + } else { + PX4_WARN("mount driver already running, run vmount stop before 'vmount test'"); + return 1; + } + } + + thread_should_exit = false; + int vmount_task = px4_task_spawn_cmd("vmount", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1900, + vmount_thread_main, + (char *const *)argv + 1); + + int counter = 0; + + while (!thread_running && vmount_task >= 0) { + px4_usleep(5000); + + if (++counter >= 100) { + break; + } + } + + if (vmount_task < 0) { + PX4_ERR("failed to start"); + return -1; + } + + return counter < 100 || thread_should_exit ? 0 : -1; + } + + if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) { + PX4_WARN("mount driver not running"); + return 0; + } + + thread_should_exit = true; + + while (thread_running) { + px4_usleep(100000); + } + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (thread_running && g_thread_data) { + + for (int i = 0; i < g_thread_data->input_objs_len; ++i) { + g_thread_data->input_objs[i]->print_status(); + } + + if (g_thread_data->input_objs_len == 0) { + PX4_INFO("Input: None"); + } + + if (g_thread_data->output_obj) { + g_thread_data->output_obj->print_status(); + + } else { + PX4_INFO("Output: None"); + } + + } else { + PX4_INFO("not running"); + } + + return 0; + } + + PX4_ERR("unrecognized command"); + usage(); + return -1; +} + +void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &got_changes) +{ + Parameters prev_params = params; + param_get(param_handles.mnt_mode_in, ¶ms.mnt_mode_in); + param_get(param_handles.mnt_mode_out, ¶ms.mnt_mode_out); + param_get(param_handles.mnt_mav_sys_id_v1, ¶ms.mnt_mav_sys_id_v1); + param_get(param_handles.mnt_mav_comp_id_v1, ¶ms.mnt_mav_comp_id_v1); + param_get(param_handles.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode); + param_get(param_handles.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode); + param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch); + param_get(param_handles.mnt_man_roll, ¶ms.mnt_man_roll); + param_get(param_handles.mnt_man_yaw, ¶ms.mnt_man_yaw); + param_get(param_handles.mnt_do_stab, ¶ms.mnt_do_stab); + param_get(param_handles.mnt_range_pitch, ¶ms.mnt_range_pitch); + param_get(param_handles.mnt_range_roll, ¶ms.mnt_range_roll); + param_get(param_handles.mnt_range_yaw, ¶ms.mnt_range_yaw); + param_get(param_handles.mnt_off_pitch, ¶ms.mnt_off_pitch); + param_get(param_handles.mnt_off_roll, ¶ms.mnt_off_roll); + param_get(param_handles.mnt_off_yaw, ¶ms.mnt_off_yaw); + param_get(param_handles.mav_sys_id, ¶ms.mav_sys_id); + param_get(param_handles.mav_comp_id, ¶ms.mav_comp_id); + param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch); + param_get(param_handles.mnt_rate_yaw, ¶ms.mnt_rate_yaw); + + got_changes = prev_params != params; +} + +bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) +{ + param_handles.mnt_mode_in = param_find("MNT_MODE_IN"); + param_handles.mnt_mode_out = param_find("MNT_MODE_OUT"); + param_handles.mnt_mav_sys_id_v1 = param_find("MNT_MAV_SYSID"); + param_handles.mnt_mav_comp_id_v1 = param_find("MNT_MAV_COMPID"); + param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE"); + param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE"); + param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH"); + param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL"); + param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW"); + param_handles.mnt_do_stab = param_find("MNT_DO_STAB"); + param_handles.mnt_range_pitch = param_find("MNT_RANGE_PITCH"); + param_handles.mnt_range_roll = param_find("MNT_RANGE_ROLL"); + param_handles.mnt_range_yaw = param_find("MNT_RANGE_YAW"); + param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH"); + param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL"); + param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW"); + param_handles.mav_sys_id = param_find("MAV_SYS_ID"); + param_handles.mav_comp_id = param_find("MAV_COMP_ID"); + param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH"); + param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW"); + + if (param_handles.mnt_mode_in == PARAM_INVALID || + param_handles.mnt_mode_out == PARAM_INVALID || + param_handles.mnt_mav_sys_id_v1 == PARAM_INVALID || + param_handles.mnt_mav_comp_id_v1 == PARAM_INVALID || + param_handles.mnt_ob_lock_mode == PARAM_INVALID || + param_handles.mnt_ob_norm_mode == PARAM_INVALID || + param_handles.mnt_man_pitch == PARAM_INVALID || + param_handles.mnt_man_roll == PARAM_INVALID || + param_handles.mnt_man_yaw == PARAM_INVALID || + param_handles.mnt_do_stab == PARAM_INVALID || + param_handles.mnt_range_pitch == PARAM_INVALID || + param_handles.mnt_range_roll == PARAM_INVALID || + param_handles.mnt_range_yaw == PARAM_INVALID || + param_handles.mnt_off_pitch == PARAM_INVALID || + param_handles.mnt_off_roll == PARAM_INVALID || + param_handles.mnt_off_yaw == PARAM_INVALID || + param_handles.mav_sys_id == PARAM_INVALID || + param_handles.mav_comp_id == PARAM_INVALID || + param_handles.mnt_rate_pitch == PARAM_INVALID || + param_handles.mnt_rate_yaw == PARAM_INVALID + ) { + return false; + } + + bool dummy; + update_params(param_handles, params, dummy); + return true; +} + +static void usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Mount (Gimbal) control driver. It maps several different input methods (eg. RC or MAVLink) to a configured +output (eg. AUX channels or MAVLink). + +Documentation how to use it is on the [gimbal_control](https://dev.px4.io/master/en/advanced/gimbal_control.html) page. + +### Implementation +Each method is implemented in its own class, and there is a common base class for inputs and outputs. +They are connected via an API, defined by the `ControlData` data structure. This makes sure that each input method +can be used with each output method and new inputs/outputs can be added with minimal effort. + +### Examples +Test the output by setting a fixed yaw angle (and the other axes to 0): +$ vmount stop +$ vmount test yaw 30 +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("vmount", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one axis (vmount must not be running)"); + PRINT_MODULE_USAGE_ARG("roll|pitch|yaw ", "Specify an axis and an angle in degrees", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} diff --git a/src/prometheus_px4/src/modules/vmount/vmount_params.c b/src/prometheus_px4/src/modules/vmount/vmount_params.c new file mode 100644 index 0000000..4687af1 --- /dev/null +++ b/src/prometheus_px4/src/modules/vmount/vmount_params.c @@ -0,0 +1,263 @@ +/**************************************************************************** +* +* 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 vmount_params.c +* @author Leon Müller (thedevleon) +* @author Matthew Edwards (mje-nz) +* +*/ + +/** +* Mount input mode +* +* RC uses the AUX input channels (see MNT_MAN_* parameters), +* MAVLINK_ROI uses the MAV_CMD_DO_SET_ROI Mavlink message, and MAVLINK_DO_MOUNT the +* MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL messages to control a mount. +* +* @value -1 DISABLED +* @value 0 AUTO +* @value 1 RC +* @value 2 MAVLINK_ROI (protocol v1) +* @value 3 MAVLINK_DO_MOUNT (protocol v1) +* @value 4 MAVlink gimbal protocol v2 +* @min -1 +* @max 4 +* @group Mount +* @reboot_required true +*/ +PARAM_DEFINE_INT32(MNT_MODE_IN, -1); + +/** +* Mount output mode +* +* AUX uses the mixer output Control Group #2. +* MAVLINK uses the MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages +* to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID) +* +* @value 0 AUX +* @value 1 MAVLink gimbal protocol v1 +* @value 2 MAVLink gimbal protocol v2 +* @min 0 +* @max 2 +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_MODE_OUT, 0); + +/** +* Mavlink System ID of the mount +* +* If MNT_MODE_OUT is MAVLink gimbal protocol v1, mount configure/control commands will be sent with this target ID. +* +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_MAV_SYSID, 1); + +/** +* Mavlink Component ID of the mount +* +* If MNT_MODE_OUT is MAVLink protocol v2, mount configure/control commands will be sent with this component ID. +* +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_MAV_COMPID, 154); + +/** +* Mixer value for selecting normal mode +* +* if required by the gimbal (only in AUX output mode) +* +* @min -1.0 +* @max 1.0 +* @decimal 3 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f); + +/** +* Mixer value for selecting a locking mode +* +* if required for the gimbal (only in AUX output mode) +* +* @min -1.0 +* @max 1.0 +* @decimal 3 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f); + +/** +* Auxiliary channel to control roll (in AUX input or manual mode). +* +* @value 0 Disable +* @value 1 AUX1 +* @value 2 AUX2 +* @value 3 AUX3 +* @value 4 AUX4 +* @value 5 AUX5 +* @value 6 AUX6 +* @min 0 +* @max 6 +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0); + +/** +* Auxiliary channel to control pitch (in AUX input or manual mode). +* +* @value 0 Disable +* @value 1 AUX1 +* @value 2 AUX2 +* @value 3 AUX3 +* @value 4 AUX4 +* @value 5 AUX5 +* @value 6 AUX6 +* @min 0 +* @max 6 +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0); + +/** +* Auxiliary channel to control yaw (in AUX input or manual mode). +* +* @value 0 Disable +* @value 1 AUX1 +* @value 2 AUX2 +* @value 3 AUX3 +* @value 4 AUX4 +* @value 5 AUX5 +* @value 6 AUX6 +* @min 0 +* @max 6 +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_MAN_YAW, 0); + +/** +* Stabilize the mount +* +* Set to true for servo gimbal, false for passthrough. +* This is required for a gimbal which is not capable of stabilizing itself +* and relies on the IMU's attitude estimation. +* +* @value 0 Disable +* @value 1 Stabilize all axis +* @value 2 Stabilize yaw for absolute/lock mode. +* @min 0 +* @max 2 +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_DO_STAB, 0); + +/** +* Range of pitch channel output in degrees (only in AUX output mode). +* +* @min 1.0 +* @max 720.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_RANGE_PITCH, 360.0f); + +/** +* Range of roll channel output in degrees (only in AUX output mode). +* +* @min 1.0 +* @max 720.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 360.0f); + +/** +* Range of yaw channel output in degrees (only in AUX output mode). +* +* @min 1.0 +* @max 720.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_RANGE_YAW, 360.0f); + +/** +* Offset for pitch channel output in degrees. +* +* @min -360.0 +* @max 360.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_OFF_PITCH, 0.0f); + +/** +* Offset for roll channel output in degrees. +* +* @min -360.0 +* @max 360.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f); + +/** +* Offset for yaw channel output in degrees. +* +* @min -360.0 +* @max 360.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f); + +/** + * Angular pitch rate for manual input in degrees/second. + * + * Full stick input [-1..1] translats to [-pitch rate..pitch rate]. + * + * @min 1.0 + * @max 90.0 + * @group Mount + */ +PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f); + +/** + * Angular yaw rate for manual input in degrees/second. + * + * Full stick input [-1..1] translats to [-yaw rate..yaw rate]. + * + * @min 1.0 + * @max 90.0 + * @group Mount + */ +PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f); diff --git a/src/prometheus_px4/src/modules/vtol_att_control/CMakeLists.txt b/src/prometheus_px4/src/modules/vtol_att_control/CMakeLists.txt new file mode 100644 index 0000000..e6316b1 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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__vtol_att_control + MAIN vtol_att_control + SRCS + vtol_att_control_main.cpp + tiltrotor.cpp + vtol_type.cpp + tailsitter.cpp + standard.cpp + ) + diff --git a/src/prometheus_px4/src/modules/vtol_att_control/standard.cpp b/src/prometheus_px4/src/modules/vtol_att_control/standard.cpp new file mode 100644 index 0000000..41e5a4a --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/standard.cpp @@ -0,0 +1,428 @@ +/**************************************************************************** + * + * 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 standard.cpp + * + * @author Simon Wilks + * @author Roman Bapst + * @author Andreas Antener + * @author Sander Smeets + * +*/ + +#include "standard.h" +#include "vtol_att_control_main.h" + +#include + +using namespace matrix; + +Standard::Standard(VtolAttitudeControl *attc) : + VtolType(attc) +{ + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + _vtol_schedule.transition_start = 0; + _pusher_active = false; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; + + _params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT"); + _params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP"); + _params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF"); + _params_handles_standard.reverse_output = param_find("VT_B_REV_OUT"); + _params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL"); +} + +void +Standard::parameters_update() +{ + float v; + + /* duration of a forwards transition to fw mode */ + param_get(_params_handles_standard.pusher_ramp_dt, &v); + _params_standard.pusher_ramp_dt = math::constrain(v, 0.0f, 20.0f); + + /* MC ramp up during back transition to mc mode */ + param_get(_params_handles_standard.back_trans_ramp, &v); + _params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params->back_trans_duration); + + _airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend; + + /* pitch setpoint offset */ + param_get(_params_handles_standard.pitch_setpoint_offset, &v); + _params_standard.pitch_setpoint_offset = math::radians(v); + + /* reverse output */ + param_get(_params_handles_standard.reverse_output, &v); + _params_standard.reverse_output = math::constrain(v, 0.0f, 1.0f); + + /* reverse output */ + param_get(_params_handles_standard.reverse_delay, &v); + _params_standard.reverse_delay = math::constrain(v, 0.0f, 10.0f); + +} + +void Standard::update_vtol_state() +{ + /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up + * forward speed. After the vehicle has picked up enough speed the rotors shutdown. + * For the back transition the pusher motor is immediately stopped and rotors reactivated. + */ + + float mc_weight = _mc_roll_weight; + float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + + if (_vtol_vehicle_status->vtol_transition_failsafe) { + // Failsafe event, engage mc motors immediately + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + _pusher_throttle = 0.0f; + _reverse_output = 0.0f; + + //reset failsafe when FW is no longer requested + if (!_attc->is_fixed_wing_requested()) { + _vtol_vehicle_status->vtol_transition_failsafe = false; + } + + } else if (!_attc->is_fixed_wing_requested()) { + + // the transition to fw mode switch is off + if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) { + // in mc mode + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + mc_weight = 1.0f; + _pusher_throttle = 0.0f; + _reverse_output = 0.0f; + + } else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { + // Regular backtransition + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC; + _vtol_schedule.transition_start = hrt_absolute_time(); + _reverse_output = _params_standard.reverse_output; + + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) { + // failsafe back to mc mode + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + mc_weight = 1.0f; + _pusher_throttle = 0.0f; + _reverse_output = 0.0f; + + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { + // transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed + + const Dcmf R_to_body(Quatf(_v_att->q).inversed()); + const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); + + float x_vel = vel(0); + + if (time_since_trans_start > _params->back_trans_duration || + (_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise) || + can_transition_on_ground()) { + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + } + } + + } else { + // the transition to fw mode switch is on + if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE || _vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { + // start transition to fw mode + /* NOTE: The failsafe transition to fixed-wing was removed because it can result in an + * unsafe flying state. */ + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_FW; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { + // in fw mode + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; + mc_weight = 0.0f; + + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) { + // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode + + const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) + && !_params->airspeed_disabled; + const bool minimum_trans_time_elapsed = time_since_trans_start > _params->front_trans_time_min; + + bool transition_to_fw = false; + + if (minimum_trans_time_elapsed) { + if (airspeed_triggers_transition) { + transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed; + + } else { + transition_to_fw = true; + } + } + + transition_to_fw |= can_transition_on_ground(); + + if (transition_to_fw) { + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; + + // don't set pusher throttle here as it's being ramped up elsewhere + _trans_finished_ts = hrt_absolute_time(); + } + } + } + + _mc_roll_weight = mc_weight; + _mc_pitch_weight = mc_weight; + _mc_yaw_weight = mc_weight; + _mc_throttle_weight = mc_weight; + + // map specific control phases to simple control modes + switch (_vtol_schedule.flight_mode) { + case vtol_mode::MC_MODE: + _vtol_mode = mode::ROTARY_WING; + break; + + case vtol_mode::FW_MODE: + _vtol_mode = mode::FIXED_WING; + break; + + case vtol_mode::TRANSITION_TO_FW: + _vtol_mode = mode::TRANSITION_TO_FW; + break; + + case vtol_mode::TRANSITION_TO_MC: + _vtol_mode = mode::TRANSITION_TO_MC; + break; + } +} + +void Standard::update_transition_state() +{ + float mc_weight = 1.0f; + float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + + VtolType::update_transition_state(); + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + + if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) { + if (_params_standard.pusher_ramp_dt <= 0.0f) { + // just set the final target throttle value + _pusher_throttle = _params->front_trans_throttle; + + } else if (_pusher_throttle <= _params->front_trans_throttle) { + // ramp up throttle to the target throttle value + _pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt; + } + + // do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed + if (_airspeed_trans_blend_margin > 0.0f && + PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && + _airspeed_validated->calibrated_airspeed_m_s > 0.0f && + _airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend && + time_since_trans_start > _params->front_trans_time_min) { + + mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) / + _airspeed_trans_blend_margin; + // time based blending when no airspeed sensor is set + + } else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) { + mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min; + mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f); + + } + + // ramp up FW_PSP_OFF + _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight); + + _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + q_sp.copyTo(_v_att_sp->q_d); + + // check front transition timeout + if (_params->front_trans_timeout > FLT_EPSILON) { + if (time_since_trans_start > _params->front_trans_timeout) { + // transition timeout occured, abort transition + _attc->quadchute("Transition timeout"); + } + } + + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { + + _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + + if (_v_control_mode->flag_control_climb_rate_enabled) { + // control backtransition deceleration using pitch. + _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + } + + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + q_sp.copyTo(_v_att_sp->q_d); + + _pusher_throttle = 0.0f; + + if (time_since_trans_start >= _params_standard.reverse_delay) { + // Handle throttle reversal for active breaking + float thrscale = (time_since_trans_start - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt); + thrscale = math::constrain(thrscale, 0.0f, 1.0f); + _pusher_throttle = thrscale * _params->back_trans_throttle; + } + + // continually increase mc attitude control as we transition back to mc mode + if (_params_standard.back_trans_ramp > FLT_EPSILON) { + mc_weight = time_since_trans_start / _params_standard.back_trans_ramp; + + } + + set_all_motor_state(motor_state::ENABLED); + + // set idle speed for MC actuators + if (!_flag_idle_mc) { + _flag_idle_mc = set_idle_mc(); + } + } + + mc_weight = math::constrain(mc_weight, 0.0f, 1.0f); + + _mc_roll_weight = mc_weight; + _mc_pitch_weight = mc_weight; + _mc_yaw_weight = mc_weight; + _mc_throttle_weight = mc_weight; +} + +void Standard::update_mc_state() +{ + VtolType::update_mc_state(); + + _pusher_throttle = VtolType::pusher_assist(); +} + +void Standard::update_fw_state() +{ + VtolType::update_fw_state(); +} + +/** + * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine + * what proportion of control should be applied to each of the control groups (mc and fw). + */ +void Standard::fill_actuator_outputs() +{ + auto &mc_in = _actuators_mc_in->control; + auto &fw_in = _actuators_fw_in->control; + + auto &mc_out = _actuators_out_0->control; + auto &fw_out = _actuators_out_1->control; + + const bool elevon_lock = (_params->elevons_mc_lock == 1); + + switch (_vtol_schedule.flight_mode) { + case vtol_mode::MC_MODE: + + // MC out = MC in + mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL]; + mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH]; + mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW]; + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; + mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = mc_in[actuator_controls_s::INDEX_LANDING_GEAR]; + + // FW out = 0, other than roll and pitch depending on elevon lock + fw_out[actuator_controls_s::INDEX_ROLL] = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_ROLL]; + fw_out[actuator_controls_s::INDEX_PITCH] = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_PITCH]; + fw_out[actuator_controls_s::INDEX_YAW] = 0; + fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; + fw_out[actuator_controls_s::INDEX_FLAPS] = 0; + fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0; + + break; + + case vtol_mode::TRANSITION_TO_FW: + + // FALLTHROUGH + case vtol_mode::TRANSITION_TO_MC: + // MC out = MC in (weighted) + mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; + mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = 0; + + // FW out = FW in, with VTOL transition controlling throttle and airbrakes + fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; + fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; + fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW]; + fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; + fw_out[actuator_controls_s::INDEX_FLAPS] = fw_in[actuator_controls_s::INDEX_FLAPS]; + fw_out[actuator_controls_s::INDEX_AIRBRAKES] = _reverse_output; + + break; + + case vtol_mode::FW_MODE: + // MC out = 0 + mc_out[actuator_controls_s::INDEX_ROLL] = 0; + mc_out[actuator_controls_s::INDEX_PITCH] = 0; + mc_out[actuator_controls_s::INDEX_YAW] = 0; + mc_out[actuator_controls_s::INDEX_THROTTLE] = 0; + mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = 0; + + // FW out = FW in + fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; + fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; + fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW]; + fw_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + fw_out[actuator_controls_s::INDEX_FLAPS] = fw_in[actuator_controls_s::INDEX_FLAPS]; + fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0; + break; + } + + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + + _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); +} + +void +Standard::waiting_on_tecs() +{ + // keep thrust from transition + _v_att_sp->thrust_body[0] = _pusher_throttle; +}; diff --git a/src/prometheus_px4/src/modules/vtol_att_control/standard.h b/src/prometheus_px4/src/modules/vtol_att_control/standard.h new file mode 100644 index 0000000..065b634 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/standard.h @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 standard.h +* VTOL with fixed multirotor motor configurations (such as quad) and a pusher +* (or puller aka tractor) motor for forward flight. +* +* @author Simon Wilks +* @author Roman Bapst +* @author Andreas Antener +* @author Sander Smeets +* +*/ + +#ifndef STANDARD_H +#define STANDARD_H +#include "vtol_type.h" +#include +#include + +class Standard : public VtolType +{ + +public: + + Standard(VtolAttitudeControl *_att_controller); + ~Standard() override = default; + + void update_vtol_state() override; + void update_transition_state() override; + void update_fw_state() override; + void update_mc_state() override; + void fill_actuator_outputs() override; + void waiting_on_tecs() override; + +private: + + struct { + float pusher_ramp_dt; + float back_trans_ramp; + float pitch_setpoint_offset; + float reverse_output; + float reverse_delay; + } _params_standard; + + struct { + param_t pusher_ramp_dt; + param_t back_trans_ramp; + param_t pitch_setpoint_offset; + param_t reverse_output; + param_t reverse_delay; + } _params_handles_standard; + + enum class vtol_mode { + MC_MODE = 0, + TRANSITION_TO_FW, + TRANSITION_TO_MC, + FW_MODE + }; + + struct { + vtol_mode flight_mode; // indicates in which mode the vehicle is in + hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) + } _vtol_schedule; + + float _pusher_throttle{0.0f}; + float _reverse_output{0.0f}; + float _airspeed_trans_blend_margin{0.0f}; + + void parameters_update() override; +}; +#endif diff --git a/src/prometheus_px4/src/modules/vtol_att_control/standard_params.c b/src/prometheus_px4/src/modules/vtol_att_control/standard_params.c new file mode 100644 index 0000000..633efa5 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/standard_params.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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 standard_params.c + * Parameters for the standard VTOL controller. + * + * @author Simon Wilks + * @author Roman Bapst + */ + + +/** + * Enable/disable usage of fixed-wing actuators in hover to generate forward force (instead of pitching down). + * + * This technique can be used to avoid the plane having to pitch down in order to move forward. + * This prevents large, negative lift values being created when facing strong winds. + * Fixed-wing forward actuators refers to puller/pusher (standard VTOL), or forward-tilt (tiltrotor VTOL). + * Only active if demaded down pitch is above VT_DWN_PITCH_MAX, and uses VT_FWD_THRUST_SC to get from + * demanded down pitch to fixed-wing actuation. + * + * @value 0 Disable FW forward actuation in hover. + * @value 1 Enable FW forward actuation in hover in altitude, position and auto modes (except LANDING). + * @value 2 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1. + * @value 3 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2. + * @value 4 Enable FW forward actuation in hover in altitude, position and auto modes. + * @value 5 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1 (except LANDING). + * @value 6 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2 (except LANDING). + * + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FWD_THRUST_EN, 0); + +/** + * Maximum allowed angle the vehicle is allowed to pitch down to generate forward force + * + * When fixed-wing forward actuation is active (see VT_FW_TRHUST_EN). + * If demanded down pitch exceeds this limmit, the fixed-wing forward actuators are used instead. + * + * @min 0.0 + * @max 45.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f); + +/** + * Fixed-wing actuator thrust scale for hover forward flight. + * + * Scale applied to the demanded down-pitch to get the fixed-wing forward actuation in hover mode. + * Only active if demaded down pitch is above VT_DWN_PITCH_MAX. + * Enabled via VT_FWD_THRUST_EN. + * + * @min 0.0 + * @max 2.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.7f); + +/** + * Back transition MC motor ramp up time + * + * This sets the duration during which the MC motors ramp up to the commanded thrust during the back transition stage. + * + * @unit s + * @min 0.0 + * @max 20.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_RAMP, 3.0f); + +/** + * Output on airbrakes channel during back transition + * + * Used for airbrakes or with ESCs that have reverse thrust enabled on a seperate channel + * Airbrakes need to be enables for your selected model/mixer + * + * @min 0 + * @max 1 + * @increment 0.01 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 0.0f); + + +/** + * Delay in seconds before applying back transition throttle + * + * Set this to a value greater than 0 to give the motor time to spin down. + * + * unit s + * @min 0 + * @max 10 + * @increment 1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_REV_DEL, 0.0f); + +/** + * Pusher throttle ramp up window + * + * Defines the time window during which the pusher throttle will be ramped up linearly to VT_F_TRANS_THR during a transition + * to fixed wing mode. Zero or negative values will produce an instant throttle rise to VT_F_TRANS_THR. + * + * @max 20 + * @increment 0.01 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_PSHER_RMP_DT, 3.0f); diff --git a/src/prometheus_px4/src/modules/vtol_att_control/tailsitter.cpp b/src/prometheus_px4/src/modules/vtol_att_control/tailsitter.cpp new file mode 100644 index 0000000..2a7cda3 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/tailsitter.cpp @@ -0,0 +1,325 @@ +/**************************************************************************** + * + * 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 tailsitter.cpp +* +* @author Roman Bapst +* @author David Vorsin +* +*/ + +#include "tailsitter.h" +#include "vtol_att_control_main.h" + +#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 +#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC + +using namespace matrix; + +Tailsitter::Tailsitter(VtolAttitudeControl *attc) : + VtolType(attc) +{ + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _flag_was_in_trans_mode = false; + _params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF"); +} + +void +Tailsitter::parameters_update() +{ + float v; + + param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v); + _params_tailsitter.fw_pitch_sp_offset = math::radians(v); +} + +void Tailsitter::update_vtol_state() +{ + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting in MC control mode, picking up + * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. + * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. + */ + + float pitch = Eulerf(Quatf(_v_att->q)).theta(); + + if (_vtol_vehicle_status->vtol_transition_failsafe) { + // Failsafe event, switch to MC mode immediately + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + + //reset failsafe when FW is no longer requested + if (!_attc->is_fixed_wing_requested()) { + _vtol_vehicle_status->vtol_transition_failsafe = false; + } + + } else if (!_attc->is_fixed_wing_requested()) { + + switch (_vtol_schedule.flight_mode) { // user switchig to MC mode + case vtol_mode::MC_MODE: + break; + + case vtol_mode::FW_MODE: + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case vtol_mode::TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + break; + + case vtol_mode::TRANSITION_BACK: + float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + + // check if we have reached pitch angle to switch to MC mode + if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) { + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + } + + break; + } + + } else { // user switchig to FW mode + + switch (_vtol_schedule.flight_mode) { + case vtol_mode::MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case vtol_mode::FW_MODE: + break; + + case vtol_mode::TRANSITION_FRONT_P1: { + + + const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) + && !_params->airspeed_disabled; + + bool transition_to_fw = false; + + if (pitch <= PITCH_TRANSITION_FRONT_P1) { + if (airspeed_triggers_transition) { + transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed; + + } else { + transition_to_fw = true; + } + } + + transition_to_fw |= can_transition_on_ground(); + + if (transition_to_fw) { + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; + } + + break; + } + + case vtol_mode::TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; + break; + } + } + + // map tailsitter specific control phases to simple control modes + switch (_vtol_schedule.flight_mode) { + case vtol_mode::MC_MODE: + _vtol_mode = mode::ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case vtol_mode::FW_MODE: + _vtol_mode = mode::FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case vtol_mode::TRANSITION_FRONT_P1: + _vtol_mode = mode::TRANSITION_TO_FW; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; + + case vtol_mode::TRANSITION_BACK: + _vtol_mode = mode::TRANSITION_TO_MC; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; + } +} + +void Tailsitter::update_transition_state() +{ + const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + + if (!_flag_was_in_trans_mode) { + _flag_was_in_trans_mode = true; + + if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { + // calculate rotation axis for transition. + _q_trans_start = Quatf(_v_att->q); + Vector3f z = -_q_trans_start.dcm_z(); + _trans_rot_axis = z.cross(Vector3f(0, 0, -1)); + + // as heading setpoint we choose the heading given by the direction the vehicle points + float yaw_sp = atan2f(z(1), z(0)); + + // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint, + // the yaw setpoint and zero roll since we want wings level transition + _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); + + // attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the + // multirotor frame + _q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0)); + + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { + // initial attitude setpoint for the transition should be with wings level + _q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); + Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0); + _trans_rot_axis = -x.cross(Vector3f(0, 0, -1)); + } + + _q_trans_sp = _q_trans_start; + } + + // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN + _q_trans_sp.normalize(); + + // tilt angle (zero if vehicle nose points up (hover)) + float cos_tilt = _q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * + _q_trans_sp(2) + _q_trans_sp(3) * _q_trans_sp(3); + cos_tilt = cos_tilt > 1.0f ? 1.0f : cos_tilt; + cos_tilt = cos_tilt < -1.0f ? -1.0f : cos_tilt; + const float tilt = acosf(cos_tilt); + + if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { + + const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration; + + if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) { + _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, + time_since_trans_start * trans_pitch_rate)) * _q_trans_start; + } + + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { + + const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration; + + if (!_flag_idle_mc) { + _flag_idle_mc = set_idle_mc(); + } + + if (tilt > 0.01f) { + _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, + time_since_trans_start * trans_pitch_rate)) * _q_trans_start; + } + } + + _v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2]; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _v_att_sp->timestamp = hrt_absolute_time(); + + const Eulerf euler_sp(_q_trans_sp); + _v_att_sp->roll_body = euler_sp.phi(); + _v_att_sp->pitch_body = euler_sp.theta(); + _v_att_sp->yaw_body = euler_sp.psi(); + + _q_trans_sp.copyTo(_v_att_sp->q_d); +} + +void Tailsitter::waiting_on_tecs() +{ + // copy the last trust value from the front transition + _v_att_sp->thrust_body[0] = _thrust_transition; +} + +void Tailsitter::update_fw_state() +{ + VtolType::update_fw_state(); + +} + +/** +* Write data to actuator output topic. +*/ +void Tailsitter::fill_actuator_outputs() +{ + auto &mc_in = _actuators_mc_in->control; + auto &fw_in = _actuators_fw_in->control; + + auto &mc_out = _actuators_out_0->control; + auto &fw_out = _actuators_out_1->control; + + mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + + if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { + mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + + /* allow differential thrust if enabled */ + if (_params->diff_thrust == 1) { + mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale; + } + + } else { + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; + } + + if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) { + fw_out[actuator_controls_s::INDEX_ROLL] = 0; + fw_out[actuator_controls_s::INDEX_PITCH] = 0; + + } else { + fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; + fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; + } + + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + + _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); +} diff --git a/src/prometheus_px4/src/modules/vtol_att_control/tailsitter.h b/src/prometheus_px4/src/modules/vtol_att_control/tailsitter.h new file mode 100644 index 0000000..c91a82d --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/tailsitter.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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 tailsitter.h +* +* @author Roman Bapst +* @author David Vorsin +* +*/ + +#ifndef TAILSITTER_H +#define TAILSITTER_H + +#include "vtol_type.h" + +#include +#include +#include + +class Tailsitter : public VtolType +{ + +public: + Tailsitter(VtolAttitudeControl *_att_controller); + ~Tailsitter() override = default; + + void update_vtol_state() override; + void update_transition_state() override; + void update_fw_state() override; + void fill_actuator_outputs() override; + void waiting_on_tecs() override; + +private: + + struct { + float fw_pitch_sp_offset; + } _params_tailsitter{}; + + struct { + param_t fw_pitch_sp_offset; + } _params_handles_tailsitter{}; + + enum class vtol_mode { + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ + }; + + struct { + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ + } _vtol_schedule; + + matrix::Quatf _q_trans_start; + matrix::Quatf _q_trans_sp; + matrix::Vector3f _trans_rot_axis; + + void parameters_update() override; + +}; +#endif diff --git a/src/prometheus_px4/src/modules/vtol_att_control/tailsitter_params.c b/src/prometheus_px4/src/modules/vtol_att_control/tailsitter_params.c new file mode 100644 index 0000000..34b17b0 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/tailsitter_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 tailsitter_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + * @author David Vorsin + */ + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @unit s + * @min 0.1 + * @max 5.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ diff --git a/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor.cpp b/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor.cpp new file mode 100644 index 0000000..b5794c6 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor.cpp @@ -0,0 +1,480 @@ +/**************************************************************************** + * + * 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 tiltrotor.cpp + * + * @author Roman Bapst + * @author Andreas Antener + * +*/ + +#include "tiltrotor.h" +#include "vtol_att_control_main.h" + +using namespace matrix; +using namespace time_literals; + +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition + +Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : + VtolType(attc) +{ + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _flag_was_in_trans_mode = false; + + _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); + _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); + _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); + _params_handles_tiltrotor.tilt_spinup = param_find("VT_TILT_SPINUP"); + _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); +} + +void +Tiltrotor::parameters_update() +{ + float v; + + /* vtol tilt mechanism position in mc mode */ + param_get(_params_handles_tiltrotor.tilt_mc, &v); + _params_tiltrotor.tilt_mc = v; + + /* vtol tilt mechanism position in transition mode */ + param_get(_params_handles_tiltrotor.tilt_transition, &v); + _params_tiltrotor.tilt_transition = v; + + /* vtol tilt mechanism position in fw mode */ + param_get(_params_handles_tiltrotor.tilt_fw, &v); + _params_tiltrotor.tilt_fw = v; + + /* vtol tilt mechanism position during motor spinup */ + param_get(_params_handles_tiltrotor.tilt_spinup, &v); + _params_tiltrotor.tilt_spinup = v; + + /* vtol front transition phase 2 duration */ + param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); + _params_tiltrotor.front_trans_dur_p2 = v; +} + +void Tiltrotor::update_vtol_state() +{ + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting rotors, picking up + * forward speed. After the vehicle has picked up enough speed the rotors are tilted + * forward completely. For the backtransition the motors simply rotate back. + */ + + if (_vtol_vehicle_status->vtol_transition_failsafe) { + // Failsafe event, switch to MC mode immediately + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + + //reset failsafe when FW is no longer requested + if (!_attc->is_fixed_wing_requested()) { + _vtol_vehicle_status->vtol_transition_failsafe = false; + } + + } else if (!_attc->is_fixed_wing_requested()) { + + // plane is in multicopter mode + switch (_vtol_schedule.flight_mode) { + case vtol_mode::MC_MODE: + break; + + case vtol_mode::FW_MODE: + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case vtol_mode::TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + break; + + case vtol_mode::TRANSITION_FRONT_P2: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + break; + + case vtol_mode::TRANSITION_BACK: + const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + const float ground_speed = sqrtf(_local_pos->vx * _local_pos->vx + _local_pos->vy * _local_pos->vy); + const bool ground_speed_below_cruise = _local_pos->v_xy_valid && (ground_speed <= _params->mpc_xy_cruise); + + if (_tilt_control <= _params_tiltrotor.tilt_mc && (time_since_trans_start > _params->back_trans_duration + || ground_speed_below_cruise)) { + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + } + + break; + } + + } else { + + switch (_vtol_schedule.flight_mode) { + case vtol_mode::MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case vtol_mode::FW_MODE: + break; + + case vtol_mode::TRANSITION_FRONT_P1: { + + float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + + const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) + && !_params->airspeed_disabled; + + bool transition_to_p2 = false; + + if (time_since_trans_start > _params->front_trans_time_min) { + if (airspeed_triggers_transition) { + transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed; + + } else { + transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition && + time_since_trans_start > _params->front_trans_time_openloop;; + } + } + + transition_to_p2 |= can_transition_on_ground(); + + if (transition_to_p2) { + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2; + _vtol_schedule.transition_start = hrt_absolute_time(); + } + + break; + } + + case vtol_mode::TRANSITION_FRONT_P2: + + // if the rotors have been tilted completely we switch to fw mode + if (_tilt_control >= _params_tiltrotor.tilt_fw) { + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; + _tilt_control = _params_tiltrotor.tilt_fw; + } + + break; + + case vtol_mode::TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = vtol_mode::FW_MODE; + break; + } + } + + // map tiltrotor specific control phases to simple control modes + switch (_vtol_schedule.flight_mode) { + case vtol_mode::MC_MODE: + _vtol_mode = mode::ROTARY_WING; + break; + + case vtol_mode::FW_MODE: + _vtol_mode = mode::FIXED_WING; + break; + + case vtol_mode::TRANSITION_FRONT_P1: + case vtol_mode::TRANSITION_FRONT_P2: + _vtol_mode = mode::TRANSITION_TO_FW; + break; + + case vtol_mode::TRANSITION_BACK: + _vtol_mode = mode::TRANSITION_TO_MC; + break; + } +} + +void Tiltrotor::update_mc_state() +{ + VtolType::update_mc_state(); + + /*Motor spin up: define the first second after arming as motor spin up time, during which + * the tilt is set to the value of VT_TILT_SPINUP. This allowes the user to set a spin up + * tilt angle in case the propellers don't spin up smootly in full upright (MC mode) position. + */ + + const int spin_up_duration_p1 = 1000_ms; // duration of 1st phase of spinup (at fixed tilt) + const int spin_up_duration_p2 = 700_ms; // duration of 2nd phase of spinup (transition from spinup tilt to mc tilt) + + // reset this timestamp while disarmed + if (!_v_control_mode->flag_armed) { + _last_timestamp_disarmed = hrt_absolute_time(); + _tilt_motors_for_startup = _params_tiltrotor.tilt_spinup > 0.01f; // spinup phase only required if spinup tilt > 0 + + } else if (_tilt_motors_for_startup) { + // leave motors tilted forward after arming to allow them to spin up easier + if (hrt_absolute_time() - _last_timestamp_disarmed > (spin_up_duration_p1 + spin_up_duration_p2)) { + _tilt_motors_for_startup = false; + } + } + + if (_tilt_motors_for_startup) { + if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) { + _tilt_control = _params_tiltrotor.tilt_spinup; + + } else { + // duration phase 2: begin to adapt tilt to multicopter tilt + float delta_tilt = (_params_tiltrotor.tilt_mc - _params_tiltrotor.tilt_spinup); + _tilt_control = _params_tiltrotor.tilt_spinup + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() - + (_last_timestamp_disarmed + spin_up_duration_p1)); + } + + _mc_yaw_weight = 0.0f; //disable yaw control during spinup + + } else { + // normal operation + _tilt_control = VtolType::pusher_assist(); + _mc_yaw_weight = 1.0f; + _v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt(); + } + +} + +void Tiltrotor::update_fw_state() +{ + VtolType::update_fw_state(); + + // make sure motors are tilted forward + _tilt_control = _params_tiltrotor.tilt_fw; +} + +void Tiltrotor::update_transition_state() +{ + VtolType::update_transition_state(); + + // copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp) + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + + _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + + float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _flag_was_in_trans_mode = true; + } + + if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { + // for the first part of the transition all rotors are enabled + set_all_motor_state(motor_state::ENABLED); + + // tilt rotors forward up to certain angle + if (_tilt_control <= _params_tiltrotor.tilt_transition) { + const float ramped_up_tilt = _params_tiltrotor.tilt_mc + + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * + time_since_trans_start / _params->front_trans_duration; + + // only allow increasing tilt (tilt in hover can already be non-zero) + _tilt_control = math::max(_tilt_control, ramped_up_tilt); + } + + // at low speeds give full weight to MC + _mc_roll_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + // reduce MC controls once the plane has picked up speed + if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && + _airspeed_validated->calibrated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + } + + if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) && + _airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend) { + _mc_roll_weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) / + (_params->transition_airspeed - _params->airspeed_blend); + } + + // without airspeed do timed weight changes + if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) && + time_since_trans_start > _params->front_trans_time_min) { + _mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) / + (_params->front_trans_time_openloop - _params->front_trans_time_min); + _mc_yaw_weight = _mc_roll_weight; + } + + _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; + + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) { + // the plane is ready to go into fixed wing mode, tilt the rotors forward completely + _tilt_control = _params_tiltrotor.tilt_transition + + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start / + _params_tiltrotor.front_trans_dur_p2; + + _mc_roll_weight = 0.0f; + _mc_yaw_weight = 0.0f; + + // ramp down motors not used in fixed-wing flight (setting MAX_PWM down scales the given output into the new range) + int ramp_down_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) * + (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN; + + + set_alternate_motor_state(motor_state::VALUE, ramp_down_value); + + + _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; + + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { + // turn on all MC motors + set_all_motor_state(motor_state::ENABLED); + + + // set idle speed for rotary wing mode + if (!_flag_idle_mc) { + _flag_idle_mc = set_idle_mc(); + } + + // tilt rotors back + if (_tilt_control > _params_tiltrotor.tilt_mc) { + _tilt_control = _params_tiltrotor.tilt_fw - + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * time_since_trans_start / 1.0f; + } + + _mc_yaw_weight = 1.0f; + + // control backtransition deceleration using pitch. + if (_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + } + + // while we quickly rotate back the motors keep throttle at idle + if (time_since_trans_start < 1.0f) { + _mc_throttle_weight = 0.0f; + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + + } else { + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + // slowly ramp up throttle to avoid step inputs + _mc_throttle_weight = (time_since_trans_start - 1.0f) / 1.0f; + } + + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + } + + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + q_sp.copyTo(_v_att_sp->q_d); + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + _mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f); +} + +void Tiltrotor::waiting_on_tecs() +{ + // keep multicopter thrust until we get data from TECS + _v_att_sp->thrust_body[0] = _thrust_transition; +} + +/** +* Write data to actuator output topic. +*/ +void Tiltrotor::fill_actuator_outputs() +{ + auto &mc_in = _actuators_mc_in->control; + auto &fw_in = _actuators_fw_in->control; + + auto &mc_out = _actuators_out_0->control; + auto &fw_out = _actuators_out_1->control; + + // Multirotor output + mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + + if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { + mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + + /* allow differential thrust if enabled */ + if (_params->diff_thrust == 1) { + mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale; + } + + } else { + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; + } + + // Fixed wing output + fw_out[4] = _tilt_control; + + if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) { + fw_out[actuator_controls_s::INDEX_ROLL] = 0; + fw_out[actuator_controls_s::INDEX_PITCH] = 0; + fw_out[actuator_controls_s::INDEX_YAW] = 0; + + } else { + fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; + fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; + fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW]; + } + + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + + _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); +} + +/* + * Increase combined thrust of MC propellers if motors are tilted. Assumes that all MC motors are tilted equally. + */ + +float Tiltrotor::thrust_compensation_for_tilt() +{ + // only compensate for tilt angle up to 0.5 * max tilt + float compensated_tilt = math::constrain(_tilt_control, 0.0f, 0.5f); + + // increase vertical thrust by 1/cos(tilt), limmit to [-1,0] + return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f); +} diff --git a/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor.h b/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor.h new file mode 100644 index 0000000..1525f2b --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 tiltrotor.h +* +* @author Roman Bapst +* +*/ + +#ifndef TILTROTOR_H +#define TILTROTOR_H +#include "vtol_type.h" +#include +#include + +class Tiltrotor : public VtolType +{ + +public: + + Tiltrotor(VtolAttitudeControl *_att_controller); + ~Tiltrotor() override = default; + + void update_vtol_state() override; + void update_transition_state() override; + void fill_actuator_outputs() override; + void update_mc_state() override; + void update_fw_state() override; + void waiting_on_tecs() override; + float thrust_compensation_for_tilt(); + +private: + + struct { + float tilt_mc; /**< actuator value corresponding to mc tilt */ + float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ + float tilt_fw; /**< actuator value corresponding to fw tilt */ + float tilt_spinup; /**< actuator value corresponding to spinup tilt */ + float front_trans_dur_p2; + } _params_tiltrotor; + + struct { + param_t tilt_mc; + param_t tilt_transition; + param_t tilt_fw; + param_t tilt_spinup; + param_t front_trans_dur_p2; + } _params_handles_tiltrotor; + + enum class vtol_mode { + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ + }; + + /** + * Specific to tiltrotor with vertical aligned rear engine/s. + * These engines need to be shut down in fw mode. During the back-transition + * they need to idle otherwise they need too much time to spin up for mc mode. + */ + + + struct { + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ + } _vtol_schedule; + + float _tilt_control{0.0f}; /**< actuator value for the tilt servo */ + + void parameters_update() override; + hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */ + bool _tilt_motors_for_startup{false}; + +}; +#endif diff --git a/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor_params.c b/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor_params.c new file mode 100644 index 0000000..31a6d54 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/tiltrotor_params.c @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 tiltrotor_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + */ + +/** + * Position of tilt servo in mc mode + * + * @min 0.0 + * @max 1.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f); + +/** + * Position of tilt servo in transition mode + * + * @min 0.0 + * @max 1.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); + +/** + * Position of tilt servo in fw mode + * + * @min 0.0 + * @max 1.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); + +/** + * Tilt actuator control value commanded when disarmed and during the first second after arming. + * + * This specific tilt during spin-up is necessary for some systems whose motors otherwise don't + * spin-up freely. + * + * @min 0.0 + * @max 1.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_SPINUP, 0.0f); + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @unit s + * @min 0.1 + * @max 5.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); diff --git a/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_main.cpp new file mode 100644 index 0000000..a9f3e2c --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -0,0 +1,525 @@ +/**************************************************************************** + * + * 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 VTOL_att_control_main.cpp + * Implementation of an attitude controller for VTOL airframes. This module receives data + * from both the fixed wing- and the multicopter attitude controllers and processes it. + * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- + * flight or transition). It also publishes the resulting controls on the actuator controls topics. + * + * @author Roman Bapst + * @author Lorenz Meier + * @author Thomas Gubler + * @author David Vorsin + * @author Sander Smeets + * @author Andreas Antener + * + */ +#include "vtol_att_control_main.h" +#include +#include + +using namespace matrix; +using namespace time_literals; + +VtolAttitudeControl::VtolAttitudeControl() : + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle")) +{ + _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ + + _params.idle_pwm_mc = PWM_DEFAULT_MIN; + _params.vtol_motor_id = 0; + + _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); + _params_handles.vtol_motor_id = param_find("VT_MOT_ID"); + _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); + _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT"); + _params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR"); + _params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P"); + _params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R"); + _params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM"); + _params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); + + _params_handles.front_trans_duration = param_find("VT_F_TRANS_DUR"); + _params_handles.back_trans_duration = param_find("VT_B_TRANS_DUR"); + _params_handles.transition_airspeed = param_find("VT_ARSP_TRANS"); + _params_handles.front_trans_throttle = param_find("VT_F_TRANS_THR"); + _params_handles.back_trans_throttle = param_find("VT_B_TRANS_THR"); + _params_handles.airspeed_blend = param_find("VT_ARSP_BLEND"); + _params_handles.airspeed_mode = param_find("FW_ARSP_MODE"); + _params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); + _params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); + _params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID"); + _params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN"); + _params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); + _params_handles.dec_to_pitch_ff = param_find("VT_B_DEC_FF"); + _params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I"); + _params_handles.back_trans_dec_sp = param_find("VT_B_DEC_MSS"); + + + _params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); + _params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); + _params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU"); + + _params_handles.vt_forward_thrust_enable_mode = param_find("VT_FWD_THRUST_EN"); + _params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1"); + _params_handles.mpc_land_alt2 = param_find("MPC_LAND_ALT2"); + + _params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); + _params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); + /* fetch initial parameter values */ + parameters_update(); + + if (static_cast(_params.vtol_type) == vtol_type::TAILSITTER) { + _vtol_type = new Tailsitter(this); + + } else if (static_cast(_params.vtol_type) == vtol_type::TILTROTOR) { + _vtol_type = new Tiltrotor(this); + + } else if (static_cast(_params.vtol_type) == vtol_type::STANDARD) { + _vtol_type = new Standard(this); + + } else { + exit_and_cleanup(); + } +} + +VtolAttitudeControl::~VtolAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +VtolAttitudeControl::init() +{ + if (!_actuator_inputs_mc.registerCallback()) { + PX4_ERR("MC actuator controls callback registration failed!"); + return false; + } + + if (!_actuator_inputs_fw.registerCallback()) { + PX4_ERR("FW actuator controls callback registration failed!"); + return false; + } + + return true; +} + +void VtolAttitudeControl::vehicle_cmd_poll() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_cmd_sub.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + + // deny any transition in auto takeoff mode, plus transition from RW to FW in land or RTL mode + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF + || (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL))) { + + result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED; + + } else { + _transition_command = int(vehicle_command.param1 + 0.5f); + _immediate_transition = (PX4_ISFINITE(vehicle_command.param2)) ? int(vehicle_command.param2 + 0.5f) : false; + } + + if (vehicle_command.from_external) { + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = vehicle_command.command; + command_ack.result = result; + command_ack.target_system = vehicle_command.source_system; + command_ack.target_component = vehicle_command.source_component; + + uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); + } + } + } +} + +/* + * Returns true if fixed-wing mode is requested. + * Changed either via switch or via command. + */ +bool +VtolAttitudeControl::is_fixed_wing_requested() +{ + bool to_fw = false; + + if (_manual_control_switches.transition_switch != manual_control_switches_s::SWITCH_POS_NONE && + _v_control_mode.flag_control_manual_enabled) { + to_fw = (_manual_control_switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON); + + } else { + // listen to transition commands if not in manual or mode switch is not mapped + to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + } + + return to_fw; +} + +void +VtolAttitudeControl::quadchute(const char *reason) +{ + if (!_vtol_vehicle_status.vtol_transition_failsafe) { + mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason); + _vtol_vehicle_status.vtol_transition_failsafe = true; + } +} + +int +VtolAttitudeControl::parameters_update() +{ + float v; + int32_t l; + /* idle pwm for mc mode */ + param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); + + /* vtol motor count */ + param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id); + param_get(_params_handles.fw_motors_off, &_params.fw_motors_off); + + /* vtol fw permanent stabilization */ + param_get(_params_handles.vtol_fw_permanent_stab, &l); + _vtol_vehicle_status.fw_permanent_stab = (l == 1); + + param_get(_params_handles.vtol_type, &l); + _params.vtol_type = l; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles.elevons_mc_lock, &l); + _params.elevons_mc_lock = (l == 1); + + /* minimum relative altitude for FW mode (QuadChute) */ + param_get(_params_handles.fw_min_alt, &v); + _params.fw_min_alt = v; + + /* maximum negative altitude error for FW mode (Adaptive QuadChute) */ + param_get(_params_handles.fw_alt_err, &v); + _params.fw_alt_err = v; + + /* maximum pitch angle (QuadChute) */ + param_get(_params_handles.fw_qc_max_pitch, &l); + _params.fw_qc_max_pitch = l; + + /* maximum roll angle (QuadChute) */ + param_get(_params_handles.fw_qc_max_roll, &l); + _params.fw_qc_max_roll = l; + + param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop); + + param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min); + + /* + * Open loop transition time needs to be larger than minimum transition time, + * anything else makes no sense and can potentially lead to numerical problems. + */ + if (_params.front_trans_time_openloop < _params.front_trans_time_min * 1.1f) { + _params.front_trans_time_openloop = _params.front_trans_time_min * 1.1f; + param_set_no_notification(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop); + mavlink_log_critical(&_mavlink_log_pub, "OL transition time set larger than min transition time"); + } + + param_get(_params_handles.front_trans_duration, &_params.front_trans_duration); + param_get(_params_handles.back_trans_duration, &_params.back_trans_duration); + param_get(_params_handles.transition_airspeed, &_params.transition_airspeed); + param_get(_params_handles.front_trans_throttle, &_params.front_trans_throttle); + param_get(_params_handles.back_trans_throttle, &_params.back_trans_throttle); + param_get(_params_handles.airspeed_blend, &_params.airspeed_blend); + param_get(_params_handles.airspeed_mode, &l); + _params.airspeed_disabled = l != 0; + param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout); + param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise); + param_get(_params_handles.diff_thrust, &_params.diff_thrust); + + param_get(_params_handles.diff_thrust_scale, &v); + _params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); + + /* maximum down pitch allowed */ + param_get(_params_handles.down_pitch_max, &v); + _params.down_pitch_max = math::radians(v); + + /* scale for fixed wing thrust used for forward acceleration in multirotor mode */ + param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale); + + // make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed + _params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f); + + param_get(_params_handles.back_trans_dec_sp, &v); + // increase the target deceleration setpoint provided to the controller by 20% + // to make overshooting the transition waypoint less likely in the presence of tracking errors + _params.back_trans_dec_sp = 1.2f * v; + + param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff); + param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i); + + param_get(_params_handles.vt_mc_on_fmu, &l); + _params.vt_mc_on_fmu = l; + + param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode); + param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1); + param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2); + + // update the parameters of the instances of base VtolType + if (_vtol_type != nullptr) { + _vtol_type->parameters_update(); + } + + return OK; +} + +void +VtolAttitudeControl::Run() +{ + if (should_exit()) { + _actuator_inputs_fw.unregisterCallback(); + _actuator_inputs_mc.unregisterCallback(); + exit_and_cleanup(); + return; + } + + const hrt_abstime now = hrt_absolute_time(); + +#if !defined(ENABLE_LOCKSTEP_SCHEDULER) + + // prevent excessive scheduling (> 500 Hz) + if (now - _last_run_timestamp < 2_ms) { + return; + } + +#endif // !ENABLE_LOCKSTEP_SCHEDULER + + _last_run_timestamp = now; + + if (!_initialized) { + parameters_update(); // initialize parameter cache + + if (_vtol_type->init()) { + _initialized = true; + + } else { + exit_and_cleanup(); + return; + } + } + + perf_begin(_loop_perf); + + const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in); + const bool updated_mc_in = _actuator_inputs_mc.update(&_actuators_mc_in); + + // run on actuator publications corresponding to VTOL mode + bool should_run = false; + + switch (_vtol_type->get_mode()) { + case mode::TRANSITION_TO_FW: + case mode::TRANSITION_TO_MC: + should_run = updated_fw_in || updated_mc_in; + break; + + case mode::ROTARY_WING: + should_run = updated_mc_in; + break; + + case mode::FIXED_WING: + should_run = updated_fw_in; + break; + } + + if (should_run) { + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + parameters_update(); + } + + _v_control_mode_sub.update(&_v_control_mode); + _manual_control_switches_sub.update(&_manual_control_switches); + _v_att_sub.update(&_v_att); + _local_pos_sub.update(&_local_pos); + _local_pos_sp_sub.update(&_local_pos_sp); + _pos_sp_triplet_sub.update(&_pos_sp_triplet); + _airspeed_validated_sub.update(&_airspeed_validated); + _tecs_status_sub.update(&_tecs_status); + _land_detected_sub.update(&_land_detected); + vehicle_cmd_poll(); + + // check if mc and fw sp were updated + bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); + bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); + + // update the vtol state machine which decides which mode we are in + _vtol_type->update_vtol_state(); + + // reset transition command if not auto control + if (_v_control_mode.flag_control_manual_enabled) { + if (_vtol_type->get_mode() == mode::ROTARY_WING) { + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + + } else if (_vtol_type->get_mode() == mode::FIXED_WING) { + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; + + } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) { + /* We want to make sure that a mode change (manual>auto) during the back transition + * doesn't result in an unsafe state. This prevents the instant fall back to + * fixed-wing on the switch from manual to auto */ + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + } + } + + + + // check in which mode we are in and call mode specific functions + switch (_vtol_type->get_mode()) { + case mode::TRANSITION_TO_FW: + case mode::TRANSITION_TO_MC: + // vehicle is doing a transition + _vtol_vehicle_status.vtol_in_trans_mode = true; + _vtol_vehicle_status.vtol_in_rw_mode = true; // making mc attitude controller work during transition + _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW); + + _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); + + if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) { + _vtol_type->update_transition_state(); + _v_att_sp_pub.publish(_v_att_sp); + } + + break; + + case mode::ROTARY_WING: + // vehicle is in rotary wing mode + _vtol_vehicle_status.vtol_in_rw_mode = true; + _vtol_vehicle_status.vtol_in_trans_mode = false; + _vtol_vehicle_status.in_transition_to_fw = false; + + _vtol_type->update_mc_state(); + _v_att_sp_pub.publish(_v_att_sp); + + break; + + case mode::FIXED_WING: + // vehicle is in fw mode + _vtol_vehicle_status.vtol_in_rw_mode = false; + _vtol_vehicle_status.vtol_in_trans_mode = false; + _vtol_vehicle_status.in_transition_to_fw = false; + + if (fw_att_sp_updated) { + _vtol_type->update_fw_state(); + _v_att_sp_pub.publish(_v_att_sp); + } + + break; + } + + _vtol_type->fill_actuator_outputs(); + _actuators_0_pub.publish(_actuators_out_0); + _actuators_1_pub.publish(_actuators_out_1); + + // Advertise/Publish vtol vehicle status + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + _vtol_vehicle_status_pub.publish(_vtol_vehicle_status); + } + + perf_end(_loop_perf); +} + +int +VtolAttitudeControl::task_spawn(int argc, char *argv[]) +{ + VtolAttitudeControl *instance = new VtolAttitudeControl(); + + 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 +VtolAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int +VtolAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_att_control is the fixed wing attitude controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_NAME("vtol_att_control", "controller"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int vtol_att_control_main(int argc, char *argv[]) +{ + return VtolAttitudeControl::main(argc, argv); +} diff --git a/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_main.h b/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_main.h new file mode 100644 index 0000000..46c8455 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_main.h @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * 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 VTOL_att_control_main.cpp + * Implementation of an attitude controller for VTOL airframes. This module receives data + * from both the fixed wing- and the multicopter attitude controllers and processes it. + * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- + * flight or transition). It also publishes the resulting controls on the actuator controls topics. + * + * @author Roman Bapst + * @author Lorenz Meier + * @author Thomas Gubler + * @author David Vorsin + * @author Sander Smeets + * @author Andreas Antener + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "standard.h" +#include "tailsitter.h" +#include "tiltrotor.h" + +using namespace time_literals; + +extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); + +class VtolAttitudeControl : public ModuleBase, public px4::WorkItem +{ +public: + + VtolAttitudeControl(); + ~VtolAttitudeControl() 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(); + + bool is_fixed_wing_requested(); + void quadchute(const char *reason); + int get_transition_command() {return _transition_command;} + bool get_immediate_transition() {return _immediate_transition;} + void reset_immediate_transition() {_immediate_transition = false;} + + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct airspeed_validated_s *get_airspeed() {return &_airspeed_validated;} + struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;} + struct tecs_status_s *get_tecs_status() {return &_tecs_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct vehicle_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;} + struct vehicle_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;} + struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;} + struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;} + + struct Params *get_params() {return &_params;} + +private: + + void Run() override; + + uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)}; + uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed subscription + uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; + uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription + uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; //manual control setpoint subscription + uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)}; + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; + uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; //vehicle attitude subscription + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription + uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::Publication _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust) + uORB::Publication _actuators_1_pub{ORB_ID(actuator_controls_1)}; + uORB::Publication _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)}; + + orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle + + vehicle_attitude_setpoint_s _v_att_sp{}; //vehicle attitude setpoint + vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint + vehicle_attitude_setpoint_s _mc_virtual_att_sp{}; // virtual mc attitude setpoint + + actuator_controls_s _actuators_fw_in{}; //actuator controls from fw_att_control + actuator_controls_s _actuators_mc_in{}; //actuator controls from mc_att_control + actuator_controls_s _actuators_out_0{}; //actuator controls going to the mc mixer + actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons) + + airspeed_validated_s _airspeed_validated{}; // airspeed + manual_control_switches_s _manual_control_switches{}; //manual control setpoint + position_setpoint_triplet_s _pos_sp_triplet{}; + tecs_status_s _tecs_status{}; + vehicle_attitude_s _v_att{}; //vehicle attitude + vehicle_control_mode_s _v_control_mode{}; //vehicle control mode + vehicle_land_detected_s _land_detected{}; + vehicle_local_position_s _local_pos{}; + vehicle_local_position_setpoint_s _local_pos_sp{}; + vtol_vehicle_status_s _vtol_vehicle_status{}; + + Params _params{}; // struct holding the parameters + + struct { + param_t idle_pwm_mc; + param_t vtol_motor_id; + param_t vtol_fw_permanent_stab; + param_t vtol_type; + param_t elevons_mc_lock; + param_t fw_min_alt; + param_t fw_alt_err; + param_t fw_qc_max_pitch; + param_t fw_qc_max_roll; + param_t front_trans_time_openloop; + param_t front_trans_time_min; + param_t front_trans_duration; + param_t back_trans_duration; + param_t transition_airspeed; + param_t front_trans_throttle; + param_t back_trans_throttle; + param_t airspeed_blend; + param_t airspeed_mode; + param_t front_trans_timeout; + param_t mpc_xy_cruise; + param_t fw_motors_off; + param_t diff_thrust; + param_t diff_thrust_scale; + param_t down_pitch_max; + param_t forward_thrust_scale; + param_t dec_to_pitch_ff; + param_t dec_to_pitch_i; + param_t back_trans_dec_sp; + param_t vt_mc_on_fmu; + param_t vt_forward_thrust_enable_mode; + param_t mpc_land_alt1; + param_t mpc_land_alt2; + } _params_handles{}; + + hrt_abstime _last_run_timestamp{0}; + + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ + int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; + bool _immediate_transition{false}; + + VtolType *_vtol_type{nullptr}; // base class for different vtol types + + bool _initialized{false}; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + void vehicle_cmd_poll(); + + int parameters_update(); //Update local parameter cache +}; diff --git a/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_params.c b/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_params.c new file mode 100644 index 0000000..f88b2a0 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/vtol_att_control_params.c @@ -0,0 +1,360 @@ +/**************************************************************************** + * + * Copyright (c) 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 vtol_att_control_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + * @author Sander Smeets + */ + +/** + * Idle speed of VTOL when in multicopter mode + * + * @unit us + * @min 900 + * @max 2000 + * @increment 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900); + +/** + * Permanent stabilization in fw mode + * + * If set to one this parameter will cause permanent attitude stabilization in fw mode. + * This parameter has been introduced for pure convenience sake. + * + * @boolean + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0); + +/** + * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) + * + * @value 0 Tailsitter + * @value 1 Tiltrotor + * @value 2 Standard + * @min 0 + * @max 2 + * @decimal 0 + * @reboot_required true + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_TYPE, 0); + +/** + * Lock elevons in multicopter mode + * + * If set to 1 the elevons are locked in multicopter mode + * + * @boolean + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 1); + +/** + * Duration of a front transition + * + * Time in seconds used for a transition + * + * @unit s + * @min 0.00 + * @max 20.00 + * @increment 1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f); + +/** + * Duration of a back transition + * + * Time in seconds used for a back transition + * + * @unit s + * @min 0.00 + * @max 20.00 + * @increment 1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 4.0f); + +/** + * Target throttle value for the transition to fixed wing flight. + * + * standard vtol: pusher + * + * tailsitter, tiltrotor: main throttle + * + * @min 0.0 + * @max 1.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_THR, 1.0f); + +/** + * Target throttle value for the transition to hover flight. + * + * standard vtol: pusher + * + * tailsitter, tiltrotor: main throttle + * + * Note for standard vtol: + * For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking + * For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking + * + * @min -1 + * @max 1 + * @increment 0.01 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f); + +/** + * Approximate deceleration during back transition + * + * The approximate deceleration during a back transition in m/s/s + * Used to calculate back transition distance in mission mode. A lower value will make the VTOL transition further from the destination waypoint. + * For standard vtol and tiltrotors a controller is used to track this value during the transition. + * + * @unit m/s^2 + * @min 0.5 + * @max 10 + * @increment 0.1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_DEC_MSS, 2.0f); + +/** + * Transition blending airspeed + * + * Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable. + * + * @unit m/s + * @min 0.00 + * @max 30.00 + * @increment 1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); + +/** + * Transition airspeed + * + * Airspeed at which we can switch to fw mode + * + * @unit m/s + * @min 0.00 + * @max 30.00 + * @increment 1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); + +/** + * Front transition timeout + * + * Time in seconds after which transition will be cancelled. Disabled if set to 0. + * + * @unit s + * @min 0.00 + * @max 30.00 + * @increment 1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f); + +/** + * Front transition minimum time + * + * Minimum time in seconds for front transition. + * + * @unit s + * @min 0.0 + * @max 20.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f); + +/** + * QuadChute Altitude + * + * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude + * the vehicle will transition back to MC mode and enter failsafe RTL + * @min 0.0 + * @max 200.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); + +/** + * Adaptive QuadChute + * + * Maximum negative altitude error for fixed wing flight. If the altitude drops below this value below the altitude setpoint + * the vehicle will transition back to MC mode and enter failsafe RTL. + * @min 0.0 + * @max 200.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f); + +/** + * QuadChute Max Pitch + * + * Maximum pitch angle before QuadChute engages + * Above this the vehicle will transition back to MC mode and enter failsafe RTL + * @min 0 + * @max 180 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_QC_P, 0); + +/** + * QuadChute Max Roll + * + * Maximum roll angle before QuadChute engages + * Above this the vehicle will transition back to MC mode and enter failsafe RTL + * @min 0 + * @max 180 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_QC_R, 0); + +/** + * Airspeed less front transition time (open loop) + * + * The duration of the front transition when there is no airspeed feedback available. + * + * @unit s + * @min 1.0 + * @max 30.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f); + +/** + * The channel number of motors that must be turned off in fixed wing mode. + * + * @min 0 + * @max 12345678 + * @increment 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); + +/** + * The channel number of motors which provide lift during hover. + * + * @min 0 + * @max 12345678 + * @increment 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_MOT_ID, 0); + +/** + * Differential thrust in forwards flight. + * + * Set to 1 to enable differential thrust in fixed-wing flight. + * + * @min 0 + * @max 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); + +/** + * Differential thrust scaling factor + * + * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f); + +/** + * Backtransition deceleration setpoint to pitch feedforward gain. + * + * + * @unit rad s^2/m + * @min 0 + * @max 0.2 + * @decimal 1 + * @increment 0.05 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_DEC_FF, 0.12f); + +/** + * Backtransition deceleration setpoint to pitch I gain. + * + * + * @unit rad s/m + * @min 0 + * @max 0.3 + * @decimal 1 + * @increment 0.05 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f); + +/** + * Enable the usage of AUX outputs for hover motors. + * + * Set this parameter to true if the vehicle's hover motors are connected to the FMU (AUX) port. + * Not required for boards that only have a FMU, and no IO. + * Only applies for standard VTOL and tiltrotor. + * + * @boolean + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_MC_ON_FMU, 0); diff --git a/src/prometheus_px4/src/modules/vtol_att_control/vtol_type.cpp b/src/prometheus_px4/src/modules/vtol_att_control/vtol_type.cpp new file mode 100644 index 0000000..50ece35 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/vtol_type.cpp @@ -0,0 +1,600 @@ +/**************************************************************************** + * + * 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 vtol_type.cpp +* +* @author Roman Bapst +* @author Andreas Antener +* +*/ + +#include "vtol_type.h" +#include "vtol_att_control_main.h" + +#include +#include +#include + +using namespace matrix; + + +VtolType::VtolType(VtolAttitudeControl *att_controller) : + _attc(att_controller), + _vtol_mode(mode::ROTARY_WING) +{ + _v_att = _attc->get_att(); + _v_att_sp = _attc->get_att_sp(); + _mc_virtual_att_sp = _attc->get_mc_virtual_att_sp(); + _fw_virtual_att_sp = _attc->get_fw_virtual_att_sp(); + _v_control_mode = _attc->get_control_mode(); + _vtol_vehicle_status = _attc->get_vtol_vehicle_status(); + _actuators_out_0 = _attc->get_actuators_out0(); + _actuators_out_1 = _attc->get_actuators_out1(); + _actuators_mc_in = _attc->get_actuators_mc_in(); + _actuators_fw_in = _attc->get_actuators_fw_in(); + _local_pos = _attc->get_local_pos(); + _local_pos_sp = _attc->get_local_pos_sp(); + _airspeed_validated = _attc->get_airspeed(); + _tecs_status = _attc->get_tecs_status(); + _land_detected = _attc->get_land_detected(); + _params = _attc->get_params(); + + for (auto &pwm_max : _max_mc_pwm_values.values) { + pwm_max = PWM_DEFAULT_MAX; + } + + for (auto &pwm_disarmed : _disarmed_pwm_values.values) { + pwm_disarmed = PWM_MOTOR_OFF; + } +} + +bool VtolType::init() +{ + const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH; + + int fd = px4_open(dev, 0); + + if (fd < 0) { + PX4_ERR("can't open %s", dev); + return false; + } + + int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values); + _current_max_pwm_values = _max_mc_pwm_values; + + if (ret != PX4_OK) { + PX4_ERR("failed getting max values"); + px4_close(fd); + return false; + } + + ret = px4_ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&_min_mc_pwm_values); + + if (ret != PX4_OK) { + PX4_ERR("failed getting min values"); + px4_close(fd); + return false; + } + + ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values); + + if (ret != PX4_OK) { + PX4_ERR("failed getting disarmed values"); + px4_close(fd); + return false; + } + + px4_close(fd); + + _main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->vtol_motor_id); + _alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->fw_motors_off); + + + // in order to get the main motors we take all motors and clear the alternate motor bits + for (int i = 0; i < 8; i++) { + if (_alternate_motor_channel_bitmap & (1 << i)) { + _main_motor_channel_bitmap &= ~(1 << i); + } + } + + return true; + +} + +void VtolType::update_mc_state() +{ + if (!_flag_idle_mc) { + _flag_idle_mc = set_idle_mc(); + } + + resetAccelToPitchPitchIntegrator(); + + VtolType::set_all_motor_state(motor_state::ENABLED); + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; +} + +void VtolType::update_fw_state() +{ + if (_flag_idle_mc) { + _flag_idle_mc = !set_idle_fw(); + } + + resetAccelToPitchPitchIntegrator(); + + VtolType::set_alternate_motor_state(motor_state::DISABLED); + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; + + // tecs didn't publish an update yet after the transition + if (_tecs_status->timestamp < _trans_finished_ts) { + _tecs_running = false; + + } else if (!_tecs_running) { + _tecs_running = true; + _tecs_running_ts = hrt_absolute_time(); + } + + // TECS didn't publish yet or the position controller didn't publish yet AFTER tecs + // only wait on TECS we're in a mode where it is actually running + if ((!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts)) + && _v_control_mode->flag_control_altitude_enabled) { + + waiting_on_tecs(); + } + + check_quadchute_condition(); +} + +void VtolType::update_transition_state() +{ + hrt_abstime t_now = hrt_absolute_time(); + _transition_dt = (float)(t_now - _last_loop_ts) / 1e6f; + _transition_dt = math::constrain(_transition_dt, 0.0001f, 0.02f); + _last_loop_ts = t_now; + + + + check_quadchute_condition(); +} + +float VtolType::update_and_get_backtransition_pitch_sp() +{ + // maximum up or down pitch the controller is allowed to demand + const float pitch_lim = 0.3f; + const Eulerf euler(Quatf(_v_att->q)); + + const float track = atan2f(_local_pos->vy, _local_pos->vx); + const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay; + + // get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number) + const float accel_error_forward = _params->back_trans_dec_sp + accel_body_forward; + + const float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ; + + float integrator_input = _params->dec_to_pitch_i * accel_error_forward; + + if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) || + (pitch_sp_new <= -pitch_lim && accel_error_forward < 0.0f)) { + integrator_input = 0.0f; + } + + _accel_to_pitch_integ += integrator_input * _transition_dt; + + + return math::constrain(pitch_sp_new, -pitch_lim, pitch_lim); +} + +bool VtolType::can_transition_on_ground() +{ + return !_v_control_mode->flag_armed || _land_detected->landed; +} + +void VtolType::check_quadchute_condition() +{ + if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition() + && !_quadchute_command_treated) { + _attc->quadchute("QuadChute by external command"); + _quadchute_command_treated = true; + _attc->reset_immediate_transition(); + + } else { + _quadchute_command_treated = false; + } + + if (!_tecs_running) { + // reset the filtered height rate and heigh rate setpoint if TECS is not running + _ra_hrate = 0.0f; + _ra_hrate_sp = 0.0f; + } + + if (_v_control_mode->flag_armed && !_land_detected->landed) { + Eulerf euler = Quatf(_v_att->q); + + // fixed-wing minimum altitude + if (_params->fw_min_alt > FLT_EPSILON) { + + if (-(_local_pos->z) < _params->fw_min_alt) { + _attc->quadchute("QuadChute: Minimum altitude breached"); + } + } + + // adaptive quadchute + if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) { + + // We use tecs for tracking in FW and local_pos_sp during transitions + if (_tecs_running) { + // 1 second rolling average + _ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50; + _ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50; + + // are we dropping while requesting significant ascend? + if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _params->fw_alt_err) && + (_ra_hrate < -1.0f) && + (_ra_hrate_sp > 1.0f)) { + + _attc->quadchute("QuadChute: loss of altitude"); + } + + } else { + const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _params->fw_alt_err); + const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f); + + if (height_error && height_rate_error) { + _attc->quadchute("QuadChute: large altitude error"); + } + } + } + + // fixed-wing maximum pitch angle + if (_params->fw_qc_max_pitch > 0) { + + if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) { + _attc->quadchute("Maximum pitch angle exceeded"); + } + } + + // fixed-wing maximum roll angle + if (_params->fw_qc_max_roll > 0) { + + if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) { + _attc->quadchute("Maximum roll angle exceeded"); + } + } + } +} + +bool VtolType::set_idle_mc() +{ + unsigned pwm_value = _params->idle_pwm_mc; + struct pwm_output_values pwm_values {}; + + for (int i = 0; i < num_outputs_max; i++) { + if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) { + pwm_values.values[i] = pwm_value; + + } else { + pwm_values.values[i] = _min_mc_pwm_values.values[i]; + } + + pwm_values.channel_count++; + } + + return apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MINIMUM); +} + +bool VtolType::set_idle_fw() +{ + struct pwm_output_values pwm_values {}; + + for (int i = 0; i < num_outputs_max; i++) { + if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) { + pwm_values.values[i] = PWM_DEFAULT_MIN; + + } else { + pwm_values.values[i] = _min_mc_pwm_values.values[i]; + } + + pwm_values.channel_count++; + } + + return apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MINIMUM); +} + +bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type) +{ + const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH; + + int fd = px4_open(dev, 0); + + if (fd < 0) { + PX4_WARN("can't open %s", dev); + return false; + } + + int ret; + + if (type == pwm_limit_type::TYPE_MINIMUM) { + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + } else { + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + } + + px4_close(fd); + + + if (ret != OK) { + PX4_DEBUG("failed setting max values"); + return false; + } + + return true; +} + +void VtolType::set_all_motor_state(const motor_state target_state, const int value) +{ + set_main_motor_state(target_state, value); + set_alternate_motor_state(target_state, value); +} + +void VtolType::set_main_motor_state(const motor_state target_state, const int value) +{ + if (_main_motor_state != target_state) { + + if (set_motor_state(target_state, _main_motor_channel_bitmap, value)) { + _main_motor_state = target_state; + } + } +} + +void VtolType::set_alternate_motor_state(const motor_state target_state, const int value) +{ + if (_alternate_motor_state != target_state) { + + if (set_motor_state(target_state, _alternate_motor_channel_bitmap, value)) { + _alternate_motor_state = target_state; + } + } +} + +bool VtolType::set_motor_state(const motor_state target_state, const int32_t channel_bitmap, const int value) +{ + switch (target_state) { + case motor_state::ENABLED: + for (int i = 0; i < num_outputs_max; i++) { + if (is_channel_set(i, channel_bitmap)) { + _current_max_pwm_values.values[i] = _max_mc_pwm_values.values[i]; + } + } + + break; + + case motor_state::DISABLED: + for (int i = 0; i < num_outputs_max; i++) { + if (is_channel_set(i, channel_bitmap)) { + _current_max_pwm_values.values[i] = _disarmed_pwm_values.values[i]; + } + } + + break; + + case motor_state::IDLE: + + for (int i = 0; i < num_outputs_max; i++) { + if (is_channel_set(i, channel_bitmap)) { + _current_max_pwm_values.values[i] = _params->idle_pwm_mc; + } + } + + break; + + case motor_state::VALUE: + for (int i = 0; i < num_outputs_max; i++) { + if (is_channel_set(i, channel_bitmap)) { + _current_max_pwm_values.values[i] = value; + } + } + + break; + } + + _current_max_pwm_values.channel_count = num_outputs_max; + + return apply_pwm_limits(_current_max_pwm_values, pwm_limit_type::TYPE_MAXIMUM); +} + +int VtolType::generate_bitmap_from_channel_numbers(const int channels) +{ + int channel_bitmap = 0; + int channel_numbers = channels; + + int tmp; + + for (int i = 0; i < num_outputs_max; ++i) { + tmp = channel_numbers % 10; + + if (tmp == 0) { + break; + } + + channel_bitmap |= 1 << (tmp - 1); + channel_numbers = channel_numbers / 10; + } + + return channel_bitmap; +} + +bool VtolType::is_channel_set(const int channel, const int bitmap) +{ + return bitmap & (1 << channel); +} + + +float VtolType::pusher_assist() +{ + // Altitude above ground is distance sensor altitude if available, otherwise local z-position + float dist_to_ground = -_local_pos->z; + + if (_local_pos->dist_bottom_valid) { + dist_to_ground = _local_pos->dist_bottom; + } + + // disable pusher assist depending on setting of forward_thrust_enable_mode: + switch (_params->vt_forward_thrust_enable_mode) { + case DISABLE: // disable in all modes + return 0.0f; + break; + + case ENABLE_WITHOUT_LAND: // disable in land mode + if (_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND + && _v_control_mode->flag_control_auto_enabled) { + return 0.0f; + } + + break; + + case ENABLE_ABOVE_MPC_LAND_ALT1: // disable if below MPC_LAND_ALT1 + if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1)) { + return 0.0f; + } + + break; + + case ENABLE_ABOVE_MPC_LAND_ALT2: // disable if below MPC_LAND_ALT2 + if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2)) { + return 0.0f; + } + + break; + + case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or in land mode + if ((_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND + && _v_control_mode->flag_control_auto_enabled) || + (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1))) { + return 0.0f; + } + + break; + + case ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND: // disable if below MPC_LAND_ALT2 or in land mode + if ((_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND + && _v_control_mode->flag_control_auto_enabled) || + (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2))) { + return 0.0f; + } + + break; + } + + // if the thrust scale param is zero or the drone is not in some position or altitude control mode, + // then the pusher-for-pitch strategy is disabled and we can return + if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled + || _v_control_mode->flag_control_altitude_enabled)) { + return 0.0f; + } + + // Do not engage pusher assist during a failsafe event (could be a problem with the fixed wing drive) + if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) { + return 0.0f; + } + + const Dcmf R(Quatf(_v_att->q)); + const Dcmf R_sp(Quatf(_v_att_sp->q_d)); + const Eulerf euler(R); + const Eulerf euler_sp(R_sp); + + // direction of desired body z axis represented in earth frame + Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + // rotate desired body z axis into new frame which is rotated in z by the current + // heading of the vehicle. we refer to this as the heading frame. + Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2)); + body_z_sp = R_yaw * body_z_sp; + body_z_sp.normalize(); + + // calculate the desired pitch seen in the heading frame + // this value corresponds to the amount the vehicle would try to pitch down + float pitch_down = atan2f(body_z_sp(0), body_z_sp(2)); + + // normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0 + float forward_thrust = 0.0f; + + // only allow pitching down up to threshold, the rest of the desired + // forward acceleration will be compensated by the pusher/tilt + if (pitch_down < -_params->down_pitch_max) { + // desired roll angle in heading frame stays the same + float roll_new = -asinf(body_z_sp(1)); + + forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale; + // limit forward actuation to [0, 0.9] + forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f); + + // return the vehicle to level position + float pitch_new = 0.0f; + + // create corrected desired body z axis in heading frame + const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f); + Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); + + // rotate the vector into a new frame which is rotated in z by the desired heading + // with respect to the earh frame. + const float yaw_error = wrap_pi(euler_sp(2) - euler(2)); + const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error); + tilt_new = R_yaw_correction * tilt_new; + + // now extract roll and pitch setpoints + _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); + _v_att_sp->roll_body = -asinf(tilt_new(1)); + + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2))); + q_sp.copyTo(_v_att_sp->q_d); + } + + return forward_thrust; + +} diff --git a/src/prometheus_px4/src/modules/vtol_att_control/vtol_type.h b/src/prometheus_px4/src/modules/vtol_att_control/vtol_type.h new file mode 100644 index 0000000..900d6d6 --- /dev/null +++ b/src/prometheus_px4/src/modules/vtol_att_control/vtol_type.h @@ -0,0 +1,321 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 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 vtol_type.h +* +* @author Roman Bapst +* @author Sander Smeets +* @author Andreas Antener +* +*/ + +#ifndef VTOL_TYPE_H +#define VTOL_TYPE_H + +#include +#include +#include + +struct Params { + int32_t idle_pwm_mc; // pwm value for idle in mc mode + int32_t vtol_motor_id; + int32_t vtol_type; + bool elevons_mc_lock; // lock elevons in multicopter mode + float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) + float fw_alt_err; // maximum negative altitude error for FW mode (Adaptive QuadChute) + float fw_qc_max_pitch; // maximum pitch angle FW mode (QuadChute) + float fw_qc_max_roll; // maximum roll angle FW mode (QuadChute) + float front_trans_time_openloop; + float front_trans_time_min; + float front_trans_duration; + float back_trans_duration; + float transition_airspeed; + float front_trans_throttle; + float back_trans_throttle; + float airspeed_blend; + bool airspeed_disabled; + float front_trans_timeout; + float mpc_xy_cruise; + int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ + int32_t diff_thrust; + float diff_thrust_scale; + float down_pitch_max; + float forward_thrust_scale; + float dec_to_pitch_ff; + float dec_to_pitch_i; + float back_trans_dec_sp; + bool vt_mc_on_fmu; + int32_t vt_forward_thrust_enable_mode; + float mpc_land_alt1; + float mpc_land_alt2; +}; + +// Has to match 1:1 msg/vtol_vehicle_status.msg +enum class mode { + TRANSITION_TO_FW = 1, + TRANSITION_TO_MC = 2, + ROTARY_WING = 3, + FIXED_WING = 4 +}; + +enum class vtol_type { + TAILSITTER = 0, + TILTROTOR, + STANDARD +}; + +enum VtolForwardActuationMode { + DISABLE = 0, + ENABLE_WITHOUT_LAND, + ENABLE_ABOVE_MPC_LAND_ALT1, + ENABLE_ABOVE_MPC_LAND_ALT2, + ENABLE_ALL_MODES, + ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND, + ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND +}; + +// these are states that can be applied to a selection of multirotor motors. +// e.g. if we need to shut off some motors after transitioning to fixed wing mode +// we can individually disable them while others might still need to be enabled to produce thrust. +// we can select the target motors via VT_FW_MOT_OFFID +enum class motor_state { + ENABLED = 0, // motor max pwm will be set to the standard max pwm value + DISABLED, // motor max pwm will be set to a value that shuts the motor off + IDLE, // motor max pwm will be set to VT_IDLE_PWM_MC + VALUE // motor max pwm will be set to a specific value provided, see set_motor_state() +}; + +/** + * @brief Used to specify if min or max pwm values should be altered + */ +enum class pwm_limit_type { + TYPE_MINIMUM = 0, + TYPE_MAXIMUM +}; + +class VtolAttitudeControl; + +class VtolType +{ +public: + + VtolType(VtolAttitudeControl *att_controller); + VtolType(const VtolType &) = delete; + VtolType &operator=(const VtolType &) = delete; + + virtual ~VtolType() = default; + + /** + * Initialise. + */ + bool init(); + + /** + * Update vtol state. + */ + virtual void update_vtol_state() = 0; + + /** + * Update transition state. + */ + virtual void update_transition_state() = 0; + + /** + * Update multicopter state. + */ + virtual void update_mc_state(); + + /** + * Update fixed wing state. + */ + virtual void update_fw_state(); + + /** + * Write control values to actuator output topics. + */ + virtual void fill_actuator_outputs() = 0; + + /** + * Special handling opportunity for the time right after transition to FW + * before TECS is running. + */ + virtual void waiting_on_tecs() {} + + /** + * Checks for fixed-wing failsafe condition and issues abort request if needed. + */ + void check_quadchute_condition(); + + /** + * Returns true if we're allowed to do a mode transition on the ground. + */ + bool can_transition_on_ground(); + + /** + * Pusher assist in hover (pusher/pull for standard VTOL, motor tilt for tiltrotor) + */ + float pusher_assist(); + + + mode get_mode() {return _vtol_mode;} + + bool was_in_trans_mode() {return _flag_was_in_trans_mode;} + + virtual void parameters_update() = 0; + +protected: + VtolAttitudeControl *_attc; + mode _vtol_mode; + + static constexpr const int num_outputs_max = 8; + + struct vehicle_attitude_s *_v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct vehicle_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint + struct vehicle_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint + struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s *_vtol_vehicle_status; + struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_rate_control + struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control + struct vehicle_local_position_s *_local_pos; + struct vehicle_local_position_setpoint_s *_local_pos_sp; + struct airspeed_validated_s *_airspeed_validated; // airspeed + struct tecs_status_s *_tecs_status; + struct vehicle_land_detected_s *_land_detected; + + struct Params *_params; + + bool _flag_idle_mc = false; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + + bool _pusher_active = false; + float _mc_roll_weight = 1.0f; // weight for multicopter attitude controller roll output + float _mc_pitch_weight = 1.0f; // weight for multicopter attitude controller pitch output + float _mc_yaw_weight = 1.0f; // weight for multicopter attitude controller yaw output + float _mc_throttle_weight = 1.0f; // weight for multicopter throttle command. Used to avoid + + // motors spinning up or cutting too fast when doing transitions. + float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only) + + float _ra_hrate = 0.0f; // rolling average on height rate for quadchute condition + float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition + + bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition + + hrt_abstime _trans_finished_ts = 0; + + bool _tecs_running = false; + hrt_abstime _tecs_running_ts = 0; + + motor_state _main_motor_state = motor_state::DISABLED; + motor_state _alternate_motor_state = motor_state::DISABLED; + + hrt_abstime _last_loop_ts = 0; + float _transition_dt = 0; + + float _accel_to_pitch_integ = 0; + + bool _quadchute_command_treated{false}; + + + /** + * @brief Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures + * that they are spinning in mc mode. + * + * @return true on success + */ + bool set_idle_mc(); + + /** + * @brief Sets mc motor minimum pwm to PWM_MIN which ensures that the + * motors stop spinning on zero throttle in fw mode. + * + * @return true on success + */ + bool set_idle_fw(); + + void set_all_motor_state(motor_state target_state, int value = 0); + + void set_main_motor_state(motor_state target_state, int value = 0); + + void set_alternate_motor_state(motor_state target_state, int value = 0); + + float update_and_get_backtransition_pitch_sp(); + +private: + + + /** + * @brief Stores the max pwm values given by the system. + */ + struct pwm_output_values _min_mc_pwm_values {}; + struct pwm_output_values _max_mc_pwm_values {}; + struct pwm_output_values _disarmed_pwm_values {}; + + struct pwm_output_values _current_max_pwm_values {}; + + int32_t _main_motor_channel_bitmap = 0; + int32_t _alternate_motor_channel_bitmap = 0; + + /** + * @brief Adjust minimum/maximum pwm values for the output channels. + * + * @param pwm_output_values Struct containing the limit values for each channel + * @param[in] type Specifies if min or max limits are adjusted. + * + * @return True on success. + */ + bool apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type); + + /** + * @brief Determines if channel is set in a bitmap. + * + * @param[in] channel The channel + * @param[in] bitmap The bitmap to check on. + * + * @return True if set, false otherwise. + */ + bool is_channel_set(const int channel, const int bitmap); + + // generates a bitmap from a number format, e.g. 1235 -> first, second, third and fifth bits should be set. + int generate_bitmap_from_channel_numbers(const int channels); + + bool set_motor_state(const motor_state target_state, const int32_t channel_bitmap, const int value); + + void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; } + +}; + +#endif