From 68276ff3455032d3b45381b90903c0ed1e80a835 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 5 Jun 2015 06:43:10 -1000 Subject: [PATCH 01/51] Back Port from Master - Changes to build on latest uavcan master with FW upload and Node ID --- makefiles/toolchain_gnu-arm-eabi.mk | 8 +- src/lib/uavcan | 2 +- src/modules/uavcan/module.mk | 9 +- src/modules/uavcan/sensors/baro.cpp | 100 ++++++++++++------- src/modules/uavcan/sensors/baro.hpp | 24 +++-- src/modules/uavcan/uavcan_main.cpp | 146 +++++++++++++++++++++++++--- src/modules/uavcan/uavcan_main.hpp | 44 ++++++++- 7 files changed, 270 insertions(+), 63 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 3b9fefb3ef..44c63dc20b 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -130,6 +130,12 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) ARCHCFLAGS = -std=gnu99 ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ +# +# Provide defaults, but allow for module override +WFRAME_LARGER_THAN ?= 1024 + + + # Generic warnings # ARCHWARNINGS = -Wall \ @@ -138,7 +144,7 @@ ARCHWARNINGS = -Wall \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ - -Wframe-larger-than=1024 \ + -Wframe-larger-than=$(WFRAME_LARGER_THAN) \ -Wpointer-arith \ -Wlogical-op \ -Wmissing-declarations \ diff --git a/src/lib/uavcan b/src/lib/uavcan index 7719f7692a..1f1679c75d 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68 +Subproject commit 1f1679c75d989420350e69339d48b53203c5e874 diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e3fd31a9a1..1d6c205586 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -39,6 +39,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 +MODULE_STACKSIZE = 3200 +WFRAME_LARGER_THAN = 1400 # Main SRCS += uavcan_main.cpp \ @@ -65,7 +67,6 @@ INCLUDE_DIRS += $(LIBUAVCAN_INC) # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS - # # libuavcan drivers for STM32 # @@ -75,6 +76,12 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 +# +# libuavcan drivers for posix +# +include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/posix/include.mk +INCLUDE_DIRS += $(LIBUAVCAN_POSIX_INC) + # # Invoke DSDL compiler # diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 576e758df5..8198d1d994 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -41,30 +41,44 @@ const char *const UavcanBarometerBridge::NAME = "baro"; -UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), -_sub_air_data(node), -_reports(nullptr) +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : + UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), + _sub_air_pressure_data(node), + _sub_air_temperature_data(node), + _reports(nullptr) { + last_temperature = 0.0f; } int UavcanBarometerBridge::init() { int res = device::CDev::init(); + if (res < 0) { return res; } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(baro_report)); - if (_reports == nullptr) - return -1; - res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + if (_reports == nullptr) { + return -1; + } + + res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); + if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } + + res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb)); + + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + return 0; } @@ -75,8 +89,9 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } while (count--) { if (_reports->get(baro_buf)) { @@ -93,47 +108,62 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case BAROIOCSMSLPRESSURE: { - if ((arg < 80000) || (arg > 120000)) { - return -EINVAL; - } else { - log("new msl pressure %u", _msl_pressure); - _msl_pressure = arg; + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + + } else { + log("new msl pressure %u", _msl_pressure); + _msl_pressure = arg; + return OK; + } + } + + case BAROIOCGMSLPRESSURE: { + return _msl_pressure; + } + + case SENSORIOCSPOLLRATE: { + // not supported yet, pretend that everything is ok return OK; } - } - case BAROIOCGMSLPRESSURE: { - return _msl_pressure; - } - case SENSORIOCSPOLLRATE: { - // not supported yet, pretend that everything is ok - return OK; - } + case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } + default: { - return CDev::ioctl(filp, cmd, arg); - } + return CDev::ioctl(filp, cmd, arg); + } } } -void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanBarometerBridge::air_temperature_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + last_temperature = msg.static_temperature; +} + +void UavcanBarometerBridge::air_pressure_sub_cb(const + uavcan::ReceivedDataStructure &msg) { baro_report report; report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = msg.static_temperature; + report.temperature = last_temperature; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar report.error_count = 0; diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 6a39eebfb6..98dc5cf42f 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -40,7 +40,8 @@ #include "sensor_bridge.hpp" #include -#include +#include +#include class RingBuffer; @@ -56,17 +57,26 @@ public: int init() override; private: - ssize_t read(struct file *filp, char *buffer, size_t buflen); + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); + void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg); + void air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanBarometerBridge *, void (UavcanBarometerBridge::*) - (const uavcan::ReceivedDataStructure &) > - AirDataCbBinder; + (const uavcan::ReceivedDataStructure &) > + AirPressureCbBinder; - uavcan::Subscriber _sub_air_data; + typedef uavcan::MethodBinder < UavcanBarometerBridge *, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + AirTemperatureCbBinder; + + uavcan::Subscriber _sub_air_pressure_data; + uavcan::Subscriber _sub_air_temperature_data; unsigned _msl_pressure = 101325; - RingBuffer *_reports; + RingBuffer *_reports; + float last_temperature; + }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ca92d09ecb..c18d6e5d1c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,25 +53,42 @@ #include #include "uavcan_main.hpp" +#include +#include + +#include + +//todo:The Inclusion of file_server_backend is killing +// #include and leaving OK undefined +#define OK 0 /** * @file uavcan_main.cpp * - * Implements basic functinality of UAVCAN node. + * Implements basic functionality of UAVCAN node. * * @author Pavel Kirienko + * David Sidrane */ /* * UavcanNode */ UavcanNode *UavcanNode::_instance; +uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance; +uavcan_posix::dynamic_node_id_server::FileEventTracer tracer; +uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; +uavcan_posix::FirmwareVersionChecker fw_version_checker; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), - _esc_controller(_node) + _esc_controller(_node), + _fileserver_backend(_node), + _node_info_retriever(_node), + _fw_upgrade_trigger(_node, fw_version_checker), + _fw_server(_node, _fileserver_backend) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -79,6 +96,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _control_topics[3] = ORB_ID(actuator_controls_3); const int res = pthread_mutex_init(&_node_mutex, nullptr); + if (res < 0) { std::abort(); } @@ -124,6 +142,7 @@ UavcanNode::~UavcanNode() // Removing the sensor bridges auto br = _sensor_bridges.getHead(); + while (br != nullptr) { auto next = br->getSibling(); delete br; @@ -135,6 +154,8 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); + delete(_server_instance); + } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -196,7 +217,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, - static_cast(run_trampoline), nullptr); + static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { warnx("start failed: %d", errno); @@ -228,8 +249,10 @@ void UavcanNode::fill_node_info() if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { hwver.major = 1; + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { hwver.major = 2; + } else { ; // All other values of HW_ARCH resolve to zero } @@ -241,6 +264,7 @@ void UavcanNode::fill_node_info() _node.setHardwareVersion(hwver); } + int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; @@ -260,6 +284,7 @@ int UavcanNode::init(uavcan::NodeID node_id) // Actuators ret = _esc_controller.init(); + if (ret < 0) { return ret; } @@ -267,26 +292,101 @@ int UavcanNode::init(uavcan::NodeID node_id) // Sensor bridges IUavcanSensorBridge::make_all(_node, _sensor_bridges); auto br = _sensor_bridges.getHead(); + while (br != nullptr) { ret = br->init(); + if (ret < 0) { warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); return ret; } + warnx("sensor bridge '%s' init ok", br->get_name()); br = br->getSibling(); } + + /* Initialize the fw version checker. + * giving it it's path + */ + + ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); + + if (ret < 0) { + return ret; + } + + + /* Start fw file server back */ + + ret = _fw_server.start(); + + if (ret < 0) { + return ret; + } + + /* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */ + + ret = storage_backend.init(UAVCAN_NODE_DB_PATH); + + if (ret < 0) { + return ret; + } + + /* Initialize trace in the UAVCAN_NODE_DB_PATH directory */ + + ret = tracer.init(UAVCAN_LOG_FILE); + + if (ret < 0) { + return ret; + } + + /* Create dynamic node id server for the Firmware updates directory */ + + _server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer); + + if (_server_instance == 0) { + return -ENOMEM; + } + + /* Initialize the dynamic node id server */ + ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id); + + if (ret < 0) { + return ret; + } + + /* Start node info retriever to fetch node info from new nodes */ + + ret = _node_info_retriever.start(); + + if (ret < 0) { + return ret; + } + + + /* Start the fw version checker */ + + ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath()); + + if (ret < 0) { + return ret; + } + + /* Start the Node */ + return _node.start(); } void UavcanNode::node_spin_once() { perf_begin(_perfcnt_node_spin_elapsed); - const int spin_res = _node.spin(uavcan::MonotonicTime()); + const int spin_res = _node.spinOnce(); + if (spin_res < 0) { warnx("node spin error %i", spin_res); } + perf_end(_perfcnt_node_spin_elapsed); } @@ -297,9 +397,11 @@ void UavcanNode::node_spin_once() int UavcanNode::add_poll_fd(int fd) { int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { errx(1, "uavcan: too many poll fds, exiting"); } + _poll_fds[_poll_fds_num] = ::pollfd(); _poll_fds[_poll_fds_num].fd = fd; _poll_fds[_poll_fds_num].events = POLLIN; @@ -312,8 +414,6 @@ int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); - const unsigned PollTimeoutMs = 50; - // XXX figure out the output count _output_count = 2; @@ -324,8 +424,8 @@ int UavcanNode::run() memset(&_outputs, 0, sizeof(_outputs)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); - if (busevent_fd < 0) - { + + if (busevent_fd < 0) { warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } @@ -354,6 +454,7 @@ int UavcanNode::run() } while (!_task_should_exit) { + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); @@ -379,9 +480,11 @@ int UavcanNode::run() if (poll_ret < 0) { log("poll error %d", errno); continue; + } else { // get controls for required topics bool controls_updated = false; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { @@ -401,8 +504,9 @@ int UavcanNode::run() if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; } + memcpy(&_outputs.output[0], &_actuator_direct.values[0], - _actuator_direct.nvalues*sizeof(float)); + _actuator_direct.nvalues * sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; } @@ -410,11 +514,14 @@ int UavcanNode::run() // can we mix? if (_test_in_progress) { memset(&_outputs, 0, sizeof(_outputs)); + if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { - _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; - _outputs.noutputs = _test_motor.motor_number+1; + _outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; + _outputs.noutputs = _test_motor.motor_number + 1; } + new_output = true; + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -451,6 +558,7 @@ int UavcanNode::run() _outputs.output[i] = 1.0f; } } + // Output to the bus _outputs.timestamp = hrt_absolute_time(); perf_begin(_perfcnt_esc_mixer_output_elapsed); @@ -509,6 +617,7 @@ UavcanNode::teardown() _control_subs[i] = -1; } } + return (_armed_sub >= 0) ? ::close(_armed_sub) : 0; } @@ -526,12 +635,14 @@ UavcanNode::subscribe() // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + // the first fd used by CAN for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } + if (unsub_groups & (1 << i)) { warnx("unsubscribe from actuator_controls_%d", i); ::close(_control_subs[i]); @@ -583,8 +694,9 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + } if (_mixers == nullptr) { _groups_required = 0; @@ -600,6 +712,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) _mixers = nullptr; _groups_required = 0; ret = -EINVAL; + } else { _mixers->groups_required(_groups_required); @@ -640,9 +753,10 @@ UavcanNode::print_info() if (_outputs.noutputs != 0) { printf("ESC output: "); - for (uint8_t i=0; i<_outputs.noutputs; i++) { - printf("%d ", (int)(_outputs.output[i]*1000)); + for (uint8_t i = 0; i < _outputs.noutputs; i++) { + printf("%d ", (int)(_outputs.output[i] * 1000)); } + printf("\n"); // ESC status @@ -653,7 +767,8 @@ UavcanNode::print_info() printf("ESC Status:\n"); printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n"); - for (uint8_t i=0; i<_outputs.noutputs; i++) { + + for (uint8_t i = 0; i < _outputs.noutputs; i++) { printf("%d\t", esc.esc[i].esc_address); printf("%3.2f\t", (double)esc.esc[i].esc_voltage); printf("%3.2f\t", (double)esc.esc[i].esc_current); @@ -669,6 +784,7 @@ UavcanNode::print_info() // Sensor bridges auto br = _sensor_bridges.getHead(); + while (br != nullptr) { printf("Sensor '%s':\n", br->get_name()); br->print_status(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 96b3ec1a60..30d0a363b7 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -47,6 +47,14 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" + +#include +#include +#include +#include +#include + + /** * @file uavcan_main.hpp * @@ -57,18 +65,40 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" +#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" +#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) - /** * A UAVCAN node. */ class UavcanNode : public device::CDev { + static constexpr unsigned MaxBitRatePerSec = 1000000; + static constexpr unsigned bitPerFrame = 148; + static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; + static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); + + static constexpr unsigned PollTimeoutMs = 10; + static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why - static constexpr unsigned RxQueueLenPerIface = 64; - static constexpr unsigned StackSize = 3000; + + /* + * This memory is reserved for uavcan to use for queuing CAN frames. + * At 1Mbit there is approximately one CAN frame every 145 uS. + * The number of buffers sets how long you can go without calling + * node_spin_xxxx. Since our task is the only one running and the + * driver will light the fd when there is a CAN frame we can nun with + * a minimum number of buffers to conserver memory. Each buffer is + * 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate + * of ~1 mS + * 1000000/200 + */ + + static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At + static constexpr unsigned StackSize = 3400; public: typedef uavcan::Node Node; @@ -117,11 +147,19 @@ private: unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer + static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer + Node _node; ///< library instance pthread_mutex_t _node_mutex; UavcanEscController _esc_controller; + + uavcan_posix::BasicFileSeverBackend _fileserver_backend; + uavcan::NodeInfoRetriever _node_info_retriever; + uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; + uavcan::BasicFileServer _fw_server; + List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; From a0f2075d5ab3322192fde9729b038e835453af6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 22:44:00 +0200 Subject: [PATCH 02/51] navigator: Decide feasibility of mission based on current position, not home --- src/modules/navigator/mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a68f4de012..e36e3fa386 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -297,10 +297,10 @@ Mission::check_dist_1wp() mission_item.nav_cmd == NAV_CMD_TAKEOFF || mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { - /* check distance from home to item */ + /* 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); + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); if (dist_to_1wp < _param_dist_1wp.get()) { _dist_1wp_ok = true; From 2f80ddd11a4c1028d3f795bc655f71ca1f3df054 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 5 Jun 2015 21:45:34 +0200 Subject: [PATCH 03/51] Correct the yaw gains to flyable values. --- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 8ab65be7b9..bd99e0e554 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -18,10 +18,11 @@ then param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 + param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 param set BAT_N_CELLS 4 fi From 8e935e6fa6917f316b7d9add7e7513c1c814a9a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 12:58:44 +0200 Subject: [PATCH 04/51] Add new stabilize mode --- msg/vehicle_status.msg | 6 ++- src/modules/commander/commander.cpp | 50 ++++++++++++++++--- src/modules/commander/px4_custom_mode.h | 42 ++++++++++++++-- .../commander/state_machine_helper.cpp | 6 +++ 4 files changed, 92 insertions(+), 12 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 07484425b3..68e4aa9dc6 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -7,7 +7,8 @@ uint8 MAIN_STATE_AUTO_LOITER = 4 uint8 MAIN_STATE_AUTO_RTL = 5 uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 -uint8 MAIN_STATE_MAX = 8 +uint8 MAIN_STATE_STAB = 8 +uint8 MAIN_STATE_MAX = 9 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -39,7 +40,8 @@ uint8 NAVIGATION_STATE_LAND = 11 # Land mode uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 -uint8 NAVIGATION_STATE_MAX = 15 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_MAX = 16 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 67e5f7a58c..6e7e6c030a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -37,9 +37,9 @@ * * @author Petri Tanskanen * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -88,7 +88,7 @@ #include #include #include - #include +#include #include #include @@ -497,6 +497,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* ACRO */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { + /* STABILIZED */ + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); @@ -514,6 +518,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* STABILIZED */ + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); + } else { /* MANUAL */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } @@ -838,6 +845,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; + main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB"; main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; @@ -851,6 +859,7 @@ int commander_thread_main(int argc, char *argv[]) const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; @@ -1737,7 +1746,10 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && - (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && + (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state == vehicle_status_s::MAIN_STATE_ACRO || + status.main_state == vehicle_status_s::MAIN_STATE_STAB || + status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1770,7 +1782,8 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { + if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) && + (status.main_state != vehicle_status_s::MAIN_STATE_STAB)) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { @@ -1939,6 +1952,7 @@ int commander_thread_main(int argc, char *argv[]) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_STAB && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status.gps_failure) || @@ -1963,6 +1977,7 @@ int commander_thread_main(int argc, char *argv[]) * if both failed we want to terminate the flight */ if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state !=vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || @@ -2290,6 +2305,17 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + + /* manual mode is stabilized already for multirotors, so switch to acro + * for any non-manual mode + */ + if (status.is_rotary_wing) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); + } else { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); + } + + } else if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_MIDDLE) { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); } else { @@ -2401,6 +2427,18 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case vehicle_status_s::NAVIGATION_STATE_STAB: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index eaf309288f..baf651f08c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -1,8 +1,41 @@ -/* - * px4_custom_mode.h +/**************************************************************************** * - * Created on: 09.08.2013 - * Author: ton + * 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 px4_custom_mode.h + * PX4 custom flight modes + * + * @author Anton Babushkin */ #ifndef PX4_CUSTOM_MODE_H_ @@ -17,6 +50,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_OFFBOARD, + PX4_CUSTOM_MAIN_MODE_STABILIZED }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a2086e9577..4d90091a1d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -299,6 +299,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta switch (new_main_state) { case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; @@ -488,6 +489,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en switch (status->main_state) { case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_STAB: case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ @@ -514,6 +516,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; + case vehicle_status_s::MAIN_STATE_STAB: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; + break; + case vehicle_status_s::MAIN_STATE_ALTCTL: status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; From 5197be67a7974afa2763b6af618e6a4f09e2c34c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 13:08:09 +0200 Subject: [PATCH 05/51] FW control: Add skeleton for distinct altitude control and position control flight modes Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- .../fw_att_control/fw_att_control_main.cpp | 26 ++++ .../fw_pos_control_l1_main.cpp | 143 +++++++++++++++++- 2 files changed, 167 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8b0d452a3e..31778e9c81 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -854,6 +854,32 @@ FixedwingAttitudeControl::task_main() _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { + + /* the pilot does not want to change direction, + * take straight attitude setpoint from position controller + */ + if (fabsf(_manual.y) < 0.01f) { + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + } else { + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + } + + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* 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(); + } + + } else if (_vcontrol_mode.flag_control_altitude_enabled) { /* * Velocity should be controlled and manual is enabled. */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a50fc53bf7..ec944c8b1d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -165,7 +165,11 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - float _hold_alt; /**< hold altitude for velocity mode */ + float _hold_alt; /**< hold altitude for altitude mode */ + float _hdg_hold_yaw; /**< hold heading for velocity mode */ + bool _hdg_hold_enabled; /**< heading hold enabled */ + struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ + struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** ¤t_positi /* reset hold altitude */ _hold_alt = _global_pos.alt; + /* reset hold yaw */ + _hdg_hold_yaw = _att.yaw; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1251,7 +1294,103 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } 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 */ + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, + heading is set to a distant waypoint */ + + const float deadBand = (60.0f/1000.0f); + const float factor = 1.0f - deadBand; + if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + /* Need to init because last loop iteration was in a different mode */ + _hold_alt = _global_pos.alt; + _hdg_hold_yaw = _att.yaw; + } + /* 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 */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + _control_mode_current = FW_POSCTRL_MODE_POSITION; + + /* Get demanded airspeed */ + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + + /* Get demanded vertical velocity from pitch control */ + static bool was_in_deadband = false; + if (_manual.x > deadBand) { + float pitch = (_manual.x - deadBand) / factor; + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + 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 = _global_pos.alt; + was_in_deadband = true; + } + tecs_update_pitch_throttle(_hold_alt, + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); + + /* heading control */ + + if (fabsf(_manual.y) < 0.01f) { + /* heading / roll is zero, lock onto current heading */ + + // XXX calculate a waypoint in some distance + // and lock on to it + + /* just switched back from non heading-hold to heading hold */ + if (!_hdg_hold_enabled) { + _hdg_hold_enabled = true; + _hdg_hold_yaw = _att.yaw; + + get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + } + + /* we have a valid heading hold position, are we too close? */ + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < 1000) { + get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + } + + math::Vector<2> prev_wp; + prev_wp(0) = (float)_hdg_hold_prev_wp.lat; + prev_wp(1) = (float)_hdg_hold_prev_wp.lon; + + math::Vector<2> curr_wp; + curr_wp(0) = (float)_hdg_hold_curr_wp.lat; + curr_wp(1) = (float)_hdg_hold_curr_wp.lon; + + /* populate l1 control setpoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + } else { + _hdg_hold_enabled = false; + } + + } else if (_control_mode.flag_control_altitude_enabled) { + /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; From 0e11f1632c2dcd2f6417ba52cafa96d497fe3030 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 May 2015 21:07:53 +0200 Subject: [PATCH 06/51] MAVLink app: send out right mode flags for new stabilized mode --- src/modules/mavlink/mavlink_messages.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1753aaec4e..a333fb8529 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -130,6 +130,12 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; + case vehicle_status_s::NAVIGATION_STATE_STAB: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; From 8b6593495c6e433b6e4c2bb6f600a76f85b3f3d2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 23 May 2015 10:20:38 +0200 Subject: [PATCH 07/51] reset waypoints when switching to fw pos_ctrl mode --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ec944c8b1d..d5360df6f0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1303,6 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; _hdg_hold_yaw = _att.yaw; + _hdg_hold_enabled = false; // this makes sure the waypoints are reset below } /* 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) { From 629002738b67c95fb1d39f2e79eea2b7999182ae Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 23 May 2015 12:29:08 +0200 Subject: [PATCH 08/51] define new mode for altitude --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d5360df6f0..10d1441f64 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -209,6 +209,7 @@ private: enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_ALTITUDE, FW_POSCTRL_MODE_OTHER } _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. @@ -1395,7 +1396,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; - if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + 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 = _global_pos.alt; } @@ -1407,7 +1408,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } } - _control_mode_current = FW_POSCTRL_MODE_POSITION; + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + From 032484bd315ee5f958f3eeb9f62ada7aec6e6772 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 23 May 2015 21:23:39 +0200 Subject: [PATCH 09/51] added takeoff protection in altitude controlled modes, code duplication cleanup --- .../fw_pos_control_l1_main.cpp | 114 ++++++++++++------ 1 file changed, 76 insertions(+), 38 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 10d1441f64..5c7a9a2a86 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -166,6 +166,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for altitude mode */ + float _ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ @@ -180,6 +181,9 @@ private: bool land_onslope; bool land_useterrain; + bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ + hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ + /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -379,6 +383,17 @@ private: /** * Control position. */ + + /** + * Do takeoff help when in altitude controlled modes + */ + void do_takeoff_help(); + + /** + * Update desired altitude base on user pitch stick input + */ + void update_desired_altitude(float dt); + bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); @@ -470,6 +485,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), _hold_alt(0.0f), + _ground_alt(0.0f), _hdg_hold_yaw(0.0f), _hdg_hold_enabled(false), _hdg_hold_prev_wp{}, @@ -482,6 +498,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _was_in_air(false), + _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -923,6 +941,41 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } +void FixedwingPositionControl::update_desired_altitude(float dt) +{ + const float deadBand = (60.0f/1000.0f); + const float factor = 1.0f - deadBand; + static bool was_in_deadband = false; + + if (_manual.x > deadBand) { + float pitch = (_manual.x - deadBand) / factor; + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + 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 = _global_pos.alt; + was_in_deadband = true; + } +} + +void FixedwingPositionControl::do_takeoff_help() +{ + const hrt_abstime delta_takeoff = 10000000; + const float throttle_threshold = 0.3f; + const float delta_alt_takeoff = 30.0f; + + /* demand 30 m above ground if user switched into this mode during takeoff */ + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { + _hold_alt = _ground_alt + delta_alt_takeoff; + } +} + bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) @@ -954,6 +1007,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no throttle limit as default */ float throttle_max = 1.0f; + /* save time when airplane is in air */ + if (!_was_in_air && !_vehicle_status.condition_landed) { + _was_in_air = true; + _time_went_in_air = hrt_absolute_time(); + _ground_alt = _global_pos.alt; + } + /* reset flag when airplane landed */ + if (_vehicle_status.condition_landed) { + _was_in_air = false; + } + if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ @@ -1298,8 +1362,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, heading is set to a distant waypoint */ - const float deadBand = (60.0f/1000.0f); - const float factor = 1.0f - deadBand; if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; @@ -1321,23 +1383,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - /* Get demanded vertical velocity from pitch control */ - static bool was_in_deadband = false; - if (_manual.x > deadBand) { - float pitch = (_manual.x - deadBand) / factor; - _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - was_in_deadband = false; - } else if (_manual.x < - deadBand) { - float pitch = (_manual.x + deadBand) / factor; - _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - 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 = _global_pos.alt; - was_in_deadband = true; - } + /* 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*/ + do_takeoff_help(); + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, @@ -1394,8 +1445,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_altitude_enabled) { /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ - const float deadBand = (60.0f/1000.0f); - const float factor = 1.0f - deadBand; 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 = _global_pos.alt; @@ -1415,23 +1464,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - /* Get demanded vertical velocity from pitch control */ - static bool was_in_deadband = false; - if (_manual.x > deadBand) { - float pitch = (_manual.x - deadBand) / factor; - _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - was_in_deadband = false; - } else if (_manual.x < - deadBand) { - float pitch = (_manual.x + deadBand) / factor; - _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - 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 = _global_pos.alt; - was_in_deadband = true; - } + /* 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*/ + do_takeoff_help(); + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, From 126ad2247ec232b522cdaf71e86b8bee85e2d720 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:00:58 -0700 Subject: [PATCH 10/51] PX4IO: Code style fixes --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 233fd3e007..e76349642f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1240,7 +1240,7 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - if (have_armed == OK) { + if (have_armed == OK) { _in_esc_calibration_mode = armed.in_esc_calibration_mode; if (armed.armed || _in_esc_calibration_mode) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; From 4594945d5737cad4f5f5db243b3e2b8980d50c13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:01:19 -0700 Subject: [PATCH 11/51] commander: Forbid override in stabilized mode --- src/modules/commander/commander.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6e7e6c030a..e3386c29f6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2437,6 +2437,8 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_termination_enabled = false; + /* override is not ok in stabilized mode */ + control_mode.flag_external_manual_override_ok = false; break; case vehicle_status_s::NAVIGATION_STATE_ALTCTL: From ad9b875aea25aa88e83c49e92f0669406e39a817 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:04:32 -0700 Subject: [PATCH 12/51] Pos control: Update symbol name --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5c7a9a2a86..17ef22b0b7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1401,7 +1401,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, - TECS_MODE_NORMAL); + tecs_status_s::TECS_MODE_NORMAL); /* heading control */ From c99489ee9d2b912ef11292678ba7ed8f1e131f95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 23:14:06 -0700 Subject: [PATCH 13/51] FW pos control: Implement hardcore throttle limiting in manual interaction mode --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 17ef22b0b7..18698f798a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1389,13 +1389,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < 0.05f) { + throttle_max = 0.0f; + } + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), @@ -1470,13 +1476,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < 0.05f) { + throttle_max = 0.0f; + } + tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), From 21fde4b3f6a32fa8f467bfdc80d9eead15fb8cf9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 May 2015 10:30:16 -0700 Subject: [PATCH 14/51] fw att control: Code style fixes --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 31778e9c81..d685631125 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1100,8 +1100,8 @@ FixedwingAttitudeControl::task_main() /* 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) + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub > 0) { From 46d969772cc672dee7ca9d0b238ed90b4abb6c51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 May 2015 11:53:34 -0700 Subject: [PATCH 15/51] Fixed wing heading hold: Engage only after wings got close to level already --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index d685631125..b7194461e6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -858,7 +858,7 @@ FixedwingAttitudeControl::task_main() /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ - if (fabsf(_manual.y) < 0.01f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 18698f798a..4e4a3356f5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1411,12 +1411,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading control */ - if (fabsf(_manual.y) < 0.01f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { /* heading / roll is zero, lock onto current heading */ - // XXX calculate a waypoint in some distance - // and lock on to it - /* just switched back from non heading-hold to heading hold */ if (!_hdg_hold_enabled) { _hdg_hold_enabled = true; From f6dc9c972760d388f740d41edb3bc060af7b6e66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 May 2015 11:54:38 -0700 Subject: [PATCH 16/51] multicopter manual attitude control: Leave some margin for yaw control --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 ++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 97d8625b02..4136107414 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -97,6 +97,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f class MulticopterAttitudeControl { @@ -831,7 +832,7 @@ MulticopterAttitudeControl::task_main() if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp.z; + _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e5ea4d1047..7a3a5a679b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -83,6 +83,7 @@ #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f #define MIN_DIST 0.01f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f /** * Multicopter position control app start / stop handling function @@ -1387,7 +1388,7 @@ MulticopterPositionControl::task_main() //Control climb rate directly if no aiding altitude controller is active if(!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = _manual.z; + _att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER); } //Construct attitude setpoint rotation matrix From 7f07e4306ae66e37cd3ce1499c6e4f9a893bbe5c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 May 2015 19:07:14 -0700 Subject: [PATCH 17/51] commander: Fix non-existing middle switch position for acro switch --- src/modules/commander/commander.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e3386c29f6..7bf67775d1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2315,9 +2315,6 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); } - } else if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_MIDDLE) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); - } else { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } From e5e4db1923f865e189c4909e0efa918ec3fb06c8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 2 Jun 2015 14:25:27 +0200 Subject: [PATCH 18/51] POSCTRL: - make sure we always intialize the waypoints on the desired path and that the previous waypoint is behind the plane - replace magic numbers with defines --- .../fw_pos_control_l1_main.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 4e4a3356f5..f1f09473eb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -95,6 +95,10 @@ static int _control_task = -1; /**< task handle for sensor task */ +#define HDG_HOLD_DIST_NEXT 3000.0f +#define HDG_HOLD_REACHED_DIST 1000.0f +#define HDG_HOLD_SET_BACK_DIST 100.0f + /** * L1 control app start / stop handling function @@ -911,14 +915,17 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa waypoint_prev.lat = _global_pos.lat; waypoint_prev.lon = _global_pos.lon; } else { - // use waypoint which is still in front of us - waypoint_prev.lat = waypoint_next.lat; - waypoint_prev.lon = waypoint_next.lon; + /* + for previous waypoint use the one still in front of us but shift it such that it is + located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us + */ + waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; } waypoint_next.valid = true; - waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)distance / 1e6; - waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)distance / 1e6; + waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; waypoint_next.alt = _hold_alt; } @@ -1424,8 +1431,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* we have a valid heading hold position, are we too close? */ if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < 1000) { - get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); } math::Vector<2> prev_wp; From 1f3c5d00e43b1aebe765290e138094b9f59d4577 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 4 Jun 2015 08:28:43 -0600 Subject: [PATCH 19/51] fix posctl heading hold --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f1f09473eb..93244b0bb4 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -912,8 +912,8 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa if (flag_init) { // on init set first waypoint to momentary position - waypoint_prev.lat = _global_pos.lat; - waypoint_prev.lon = _global_pos.lon; + waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; } else { /* for previous waypoint use the one still in front of us but shift it such that it is From ef6e07fc9bf6f4cdf51f74c930b57d8becad185d Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 5 Jun 2015 16:01:52 +0200 Subject: [PATCH 20/51] fixed wing position control: - only lock on yaw if yawrate is below threshold - remove more magic numbers --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 93244b0bb4..1b4d7a2e15 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -94,10 +94,13 @@ #include static int _control_task = -1; /**< task handle for sensor task */ +#define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode +#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode +#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane +#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode +#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading -#define HDG_HOLD_DIST_NEXT 3000.0f -#define HDG_HOLD_REACHED_DIST 1000.0f -#define HDG_HOLD_SET_BACK_DIST 100.0f +#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes /** @@ -1398,7 +1401,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; - if (fabsf(_manual.z) < 0.05f) { + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } @@ -1418,7 +1421,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading control */ - if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { + if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH) { /* heading / roll is zero, lock onto current heading */ /* just switched back from non heading-hold to heading hold */ @@ -1482,7 +1485,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; - if (fabsf(_manual.z) < 0.05f) { + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } From e9634fbe47e41b12bb8751d52f43d648c0f6b704 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 5 Jun 2015 21:18:25 -0600 Subject: [PATCH 21/51] suppress preflight check failure for HIL autostart_ids --- src/modules/commander/commander.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 67e5f7a58c..acf41e741d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1134,11 +1134,19 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); - if (!status.condition_system_sensors_initialized) { - set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune - } - else { + param_get(_param_autostart_id, &autostart_id); + if (autostart_id > 1999) { + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); + if (!status.condition_system_sensors_initialized) { + set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune + } + else { + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } + } else { + // HIL configuration selected: real sensors will be disabled + warnx("autostart_id: %d", autostart_id); + status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } @@ -1311,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) /* provide RC and sensor status feedback to the user */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2781,7 +2789,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); From 9588e6fc6875c934aeb3e7b6de8af3a609e0d5ed Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 5 Jun 2015 21:23:35 -0600 Subject: [PATCH 22/51] whitespace --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index acf41e741d..c9af11920d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) /* provide RC and sensor status feedback to the user */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2789,7 +2789,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); From 6ed43cb3a4d36c15345332652850670215914cd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 09:45:07 +0200 Subject: [PATCH 23/51] commander: Allow to disarm via switch in HIL --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c2816fd93f..ca4a30196b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1400,7 +1400,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { + if (safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); @@ -1411,7 +1411,7 @@ int commander_thread_main(int argc, char *argv[]) } } - //Notify the user if the status of the safety switch changes + /* notify the user if the status of the safety switch changes */ if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { if (safety.safety_off) { From 7d2536f2dd4295726b1aae4e1f26b2dddc68c487 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 09:45:50 +0200 Subject: [PATCH 24/51] ROMFS: Allocate only 12K buffer for sdlog, now that we do only log at 100 Hz --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 6bfbfea396..5d374745a1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -11,7 +11,7 @@ then then fi else - if sdlog2 start -r 100 -a -b 22 -t + if sdlog2 start -r 100 -a -b 12 -t then fi fi From 9af8ba49ac6a8c43fb9a782c3bb0a2d91ea75e10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 11:35:09 +0200 Subject: [PATCH 25/51] HIL fix: Also publish filtered airspeed --- src/modules/mavlink/mavlink_receiver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 834c8398a1..7ede93ec2d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1292,6 +1292,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.baro_timestamp = timestamp; hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa; hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ From 3e0c14dea28d6b5fec3eaa05ab9681ef82545159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 11:35:29 +0200 Subject: [PATCH 26/51] MAVLink FTP: Do not list hidden directories by default --- src/modules/mavlink/mavlink_ftp.cpp | 5 +++-- src/modules/mavlink/mavlink_ftp.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 42cabb4ba6..54cb1b186a 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -299,7 +299,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) /// @brief Responds to a List command MavlinkFTP::ErrorCode -MavlinkFTP::_workList(PayloadHeader* payload) +MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden) { char dirPath[kMaxDataLength]; strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); @@ -375,7 +375,8 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: - if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + if ((!list_hidden && (strncmp(entry.d_name, ".", 1) == 0)) || + strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back direntType = kDirentSkip; } else { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 33b8996f71..998c6b3cc6 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -130,7 +130,7 @@ private: 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 _workList(PayloadHeader *payload, bool list_hidden = false); ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id); From b7986e6fdd103064128d0933f7cb32ab4252159b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 12:43:45 +0200 Subject: [PATCH 27/51] land detector: Improve performance for fixed wing setups --- .../land_detector/FixedwingLandDetector.cpp | 14 ++++++---- src/modules/land_detector/LandDetector.h | 2 +- .../land_detector/land_detector_params.c | 27 ++++++++++++++++--- 3 files changed, 34 insertions(+), 9 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index e26954d1a6..6e8393617e 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -84,11 +84,15 @@ bool FixedwingLandDetector::update() const uint64_t now = hrt_absolute_time(); bool landDetected = false; - // TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + } + + if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + } // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index b2984003b9..68f96288cd 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -87,7 +87,7 @@ protected: virtual void initialize() = 0; /** - * @brief Convinience function for polling uORB subscriptions + * @brief Convenience function for polling uORB subscriptions * @return true if there was new data and it was successfully copied **/ bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 9cf57b5fc9..b670dcc035 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -45,6 +45,8 @@ * * Maximum vertical velocity allowed to trigger a land (m/s up and down) * + * @unit m/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); @@ -54,6 +56,8 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); * * Maximum horizontal velocity allowed to trigger a land (m/s) * + * @unit m/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); @@ -63,6 +67,8 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); * * Maximum allowed around each axis to trigger a land (degrees per second) * + * @unit deg/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); @@ -72,6 +78,9 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); * * Maximum actuator output on throttle before triggering a land * + * @min 0.1 + * @max 0.5 + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); @@ -81,24 +90,36 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); * * Maximum horizontal velocity allowed to trigger a land (m/s) * + * @min 0.5 + * @max 10 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.40f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f); /** * Fixedwing max climb rate * * Maximum vertical velocity allowed to trigger a land (m/s up and down) * + * @min 5 + * @max 20 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); /** * Airspeed max * * Maximum airspeed allowed to trigger a land (m/s) * + * @min 4 + * @max 20 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f); From ab61ebca2afa49936802c86abeb9547f8685f336 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 10:53:23 +0200 Subject: [PATCH 28/51] Revert "commander: Allow to disarm via switch in HIL" This reverts commit 6ed43cb3a4d36c15345332652850670215914cd2. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ca4a30196b..c2816fd93f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1400,7 +1400,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); @@ -1411,7 +1411,7 @@ int commander_thread_main(int argc, char *argv[]) } } - /* notify the user if the status of the safety switch changes */ + //Notify the user if the status of the safety switch changes if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { if (safety.safety_off) { From 05f935cd77355ffde9e91fe340ffa6b773df09c7 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Jun 2015 07:56:48 -0600 Subject: [PATCH 29/51] inhibit more sensor checks --- src/modules/commander/commander.cpp | 13 ++++++++++--- src/modules/commander/state_machine_helper.cpp | 3 ++- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c2816fd93f..d48cccd3ed 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1154,7 +1154,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { // HIL configuration selected: real sensors will be disabled - warnx("autostart_id: %d", autostart_id); + warnx("HIL configuration; autostart_id: %d, onboard IMU sensors disabled", autostart_id); status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } @@ -1327,8 +1327,15 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + if (autostart_id > 1999) { + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + } else { + /* HIL configuration: check only RC input */ + warnx("new telemetry link connected: checking RC status"); + (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + } } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4d90091a1d..92f73ca21f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -125,7 +125,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s int prearm_ret = OK; /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = prearm_check(status, mavlink_fd); } From 02efa5a24cfc9bf4a51c922526856c9a205cb177 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:13:35 +0200 Subject: [PATCH 30/51] commander: Better text feedback --- src/modules/commander/commander.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d48cccd3ed..2b6933c27a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1695,7 +1695,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.gps_failure && !gpsIsNoisy) { status.gps_failure = false; status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); + mavlink_log_critical(mavlink_fd, "gps fix regained"); } } else if (!status.gps_failure) { @@ -1729,12 +1729,12 @@ int commander_thread_main(int argc, char *argv[]) if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + mavlink_log_critical(mavlink_fd, "Flight termination active"); } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination @@ -1744,7 +1744,7 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_info(mavlink_fd, "Detected RC signal first time"); + mavlink_log_info(mavlink_fd, "Detected radio control"); status_changed = true; } else { From d40f94bf26a89c349f5d7633434d3721b4720170 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 7 Jun 2015 22:27:31 +0200 Subject: [PATCH 31/51] fixed wing posctrl: - lock desired yaw once yaw speed is small --- .../fw_pos_control_l1_main.cpp | 62 +++++++++++-------- 1 file changed, 37 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1b4d7a2e15..733cdc51d3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -176,6 +176,7 @@ private: float _ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ + bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** ¤t_positi _hold_alt = _global_pos.alt; _hdg_hold_yaw = _att.yaw; _hdg_hold_enabled = false; // this makes sure the waypoints are reset below + _yaw_lock_engaged = 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) { @@ -1421,38 +1424,47 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading control */ - if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH && fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH) { + if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ + if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + // little yaw movement, lock to current heading + _yaw_lock_engaged = true; - /* just switched back from non heading-hold to heading hold */ - if (!_hdg_hold_enabled) { - _hdg_hold_enabled = true; - _hdg_hold_yaw = _att.yaw; - - get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); } - /* we have a valid heading hold position, are we too close? */ - if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { - get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + 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 = _att.yaw; + + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + } + + /* we have a valid heading hold position, are we too close? */ + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + } + + math::Vector<2> prev_wp; + prev_wp(0) = (float)_hdg_hold_prev_wp.lat; + prev_wp(1) = (float)_hdg_hold_prev_wp.lon; + + math::Vector<2> curr_wp; + curr_wp(0) = (float)_hdg_hold_curr_wp.lat; + curr_wp(1) = (float)_hdg_hold_curr_wp.lon; + + /* populate l1 control setpoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); } - - math::Vector<2> prev_wp; - prev_wp(0) = (float)_hdg_hold_prev_wp.lat; - prev_wp(1) = (float)_hdg_hold_prev_wp.lon; - - math::Vector<2> curr_wp; - curr_wp(0) = (float)_hdg_hold_curr_wp.lat; - curr_wp(1) = (float)_hdg_hold_curr_wp.lon; - - /* populate l1 control setpoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); } else { _hdg_hold_enabled = false; + _yaw_lock_engaged = false; } } else if (_control_mode.flag_control_altitude_enabled) { From aec4f359acd7320ff3193b46e41e0c6b2843adcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:06:26 +0200 Subject: [PATCH 32/51] Caipi config: Fix maintainer --- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 5a7d73c060..d984035675 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -2,7 +2,7 @@ # # TBS Caipirinha # -# Thomas Gubler +# Lorenz Meier # sh /etc/init.d/rc.fw_defaults @@ -29,6 +29,7 @@ then param set FW_RR_I 0.01 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 + param set SYS_COMPANION 157600 fi set MIXER caipi From 6309aa612be30e4b98dbec7d4dde5068bce17839 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:06:48 +0200 Subject: [PATCH 33/51] MAVLink app: Introduce OSD mode --- src/modules/mavlink/mavlink_main.cpp | 13 ++++++++++++- src/modules/mavlink/mavlink_main.h | 3 ++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 348185e693..5cba6c644e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1295,6 +1295,8 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_ONBOARD; } else if (strcmp(optarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "osd") == 0) { + _mode = MAVLINK_MODE_OSD; } break; @@ -1451,7 +1453,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); configure_stream("HIGHRES_IMU", 50.0f); - configure_stream("VFR_HUD", 5.0f); configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); @@ -1468,6 +1469,16 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); break; + case MAVLINK_MODE_OSD: + configure_stream("SYS_STATUS", 5.0f); + configure_stream("ATTITUDE", 30.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("BATTERY_STATUS", 1.0f); + configure_stream("SYSTEM_TIME", 1.0f); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 71749e2923..cf95c4d40c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -128,7 +128,8 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_ONBOARD + MAVLINK_MODE_ONBOARD, + MAVLINK_MODE_OSD }; void set_mode(enum MAVLINK_MODE); From 0083d6e732eb61bbe6075eb70475c25d32362a32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:07:03 +0200 Subject: [PATCH 34/51] systemlib: Update system param names --- src/modules/systemlib/system_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 17ce65d13c..275d6dca45 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -88,9 +88,9 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); * Companion computer interface * * Configures the baud rate of the companion computer interface. -* Set to zero to disable, set to 921600 to enable. -* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for -* other baud rates. +* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!) +* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1. +* 157600: enables OSD mode at 57600 baud, 8N1. * * @min 0 * @max 921600 From f22fdc5b0b65d17bf7a030a2726f02415cdbc1ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Jun 2015 22:07:23 +0200 Subject: [PATCH 35/51] ROMFS: Support for new autostart IDs --- ROMFS/px4fmu_common/init.d/rcS | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2dfe3d8e30..13b35313e7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -484,6 +484,10 @@ then then mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 fi + if param compare SYS_COMPANION 157600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 + fi fi # UAVCAN From c798b1165aa9919409c7ba5d7c339c5c754dd990 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 11:25:25 +0200 Subject: [PATCH 36/51] MAVLink app: Complete OSD config --- src/modules/mavlink/mavlink_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5cba6c644e..872406775b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1471,12 +1471,14 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_OSD: configure_stream("SYS_STATUS", 5.0f); - configure_stream("ATTITUDE", 30.0f); + configure_stream("ATTITUDE", 25.0f); + configure_stream("VFR_HUD", 5.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 10.0f); configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("BATTERY_STATUS", 1.0f); configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("RC_CHANNELS_RAW", 5.0f); break; default: From cb2ddbe57ba937625a7de0cf039b22f3ef9437ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 14:28:53 +0200 Subject: [PATCH 37/51] Caipi Mixer: Fix directions --- ROMFS/px4fmu_common/mixers/caipi.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/caipi.main.mix b/ROMFS/px4fmu_common/mixers/caipi.main.mix index cfc49ff9f4..e9d730449a 100644 --- a/ROMFS/px4fmu_common/mixers/caipi.main.mix +++ b/ROMFS/px4fmu_common/mixers/caipi.main.mix @@ -25,13 +25,13 @@ for the elevons. M: 2 O: 10000 10000 -3000 -10000 10000 -S: 0 0 -9000 -9000 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 8000 8000 0 -10000 10000 +S: 0 1 9000 9000 0 -10000 10000 M: 2 O: 10000 10000 3000 -10000 10000 -S: 0 0 -9000 -9000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 8000 8000 0 -10000 10000 +S: 0 1 -9000 -9000 0 -10000 10000 Output 2 -------- From 2903e350a74348653fa370dec3747c4dba02bd40 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 14:45:48 +0200 Subject: [PATCH 38/51] Caipi: Fix mixer and reverse params --- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 2 ++ ROMFS/px4fmu_common/mixers/caipi.main.mix | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index d984035675..1fd96d6d3d 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -30,6 +30,8 @@ then param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 param set SYS_COMPANION 157600 + param set PWM_MAIN_REV0 1 + param set PWM_MAIN_REV1 1 fi set MIXER caipi diff --git a/ROMFS/px4fmu_common/mixers/caipi.main.mix b/ROMFS/px4fmu_common/mixers/caipi.main.mix index e9d730449a..af053deab4 100644 --- a/ROMFS/px4fmu_common/mixers/caipi.main.mix +++ b/ROMFS/px4fmu_common/mixers/caipi.main.mix @@ -24,12 +24,12 @@ The scaling factor for roll inputs is adjusted to implement differential travel for the elevons. M: 2 -O: 10000 10000 -3000 -10000 10000 +O: 10000 10000 3000 -10000 10000 S: 0 0 8000 8000 0 -10000 10000 S: 0 1 9000 9000 0 -10000 10000 M: 2 -O: 10000 10000 3000 -10000 10000 +O: 10000 10000 -3000 -10000 10000 S: 0 0 8000 8000 0 -10000 10000 S: 0 1 -9000 -9000 0 -10000 10000 From 9bbb315144f6b27cb266763c1760f7095c671b7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 15:19:41 +0200 Subject: [PATCH 39/51] commander: Print home position --- src/modules/commander/commander.cpp | 28 ++++++++++++---------------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2b6933c27a..7b92942b33 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -184,6 +184,7 @@ static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; +static struct home_position_s _home; /** * The daemon app only briefly exists to start @@ -376,6 +377,8 @@ void print_status() warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no"); warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt); + warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -957,8 +960,7 @@ int commander_thread_main(int argc, char *argv[]) /* home position */ orb_advert_t home_pub = -1; - struct home_position_s home; - memset(&home, 0, sizeof(home)); + memset(&_home, 0, sizeof(_home)); /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = -1; @@ -1955,7 +1957,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &home_pub)) { status_changed = true; } } @@ -2017,12 +2019,12 @@ int commander_thread_main(int argc, char *argv[]) //First time home position update if (!status.condition_home_position_valid) { - commander_set_home_position(home_pub, home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position); } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { - commander_set_home_position(home_pub, home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position); } /* print new state */ @@ -2039,14 +2041,6 @@ int commander_thread_main(int argc, char *argv[]) mission_result.finished, mission_result.stay_in_failsafe); - // TODO handle mode changes by commands - if (main_state_changed) { - status_changed = true; - warnx("main state: %s", main_states_str[status.main_state]); - mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); - main_state_changed = false; - } - if (status.failsafe != failsafe_old) { status_changed = true; @@ -2060,10 +2054,12 @@ int commander_thread_main(int argc, char *argv[]) failsafe_old = status.failsafe; } - if (nav_state_changed) { + // TODO handle mode changes by commands + if (main_state_changed || nav_state_changed) { status_changed = true; - warnx("nav state: %s", nav_states_str[status.nav_state]); - mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); + warnx("main state: %s nav state: %s", main_states_str[status.main_state], nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]); + main_state_changed = false; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ From ac215fe2cba70e32af884f23d789ce6b8f6075c3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 8 Jun 2015 15:47:40 +0200 Subject: [PATCH 40/51] allow to give away some thrust for yaw control --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 4d9d60c2d8..aa0a8e7418 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -312,6 +312,9 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) } } else if(out > 1.0f) { + // allow to reduce thrust to get some yaw response + float thrust_reduction = fminf(0.15f, out - 1.0f); + thrust -= thrust_reduction; yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale; if(status_reg != NULL) { From c7be59038c0aafeb25c32159bf850bd69fc47a79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 10:16:24 +0200 Subject: [PATCH 41/51] L1 pos control: Publish timestamp when setting nav capabilities --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1b4d7a2e15..b882cc08c4 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -900,6 +900,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c void FixedwingPositionControl::navigation_capabilities_publish() { + _nav_capabilities.timestamp = hrt_absolute_time(); + if (_nav_capabilities_pub > 0) { orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); } else { From 0f3438eb17a9222cbe43e640269fa5fe739b49f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 10:16:51 +0200 Subject: [PATCH 42/51] Navigator: Obey minimum turn radius the controller is capabable of. --- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d2acce7899..83f3487a00 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -144,7 +144,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } - float get_acceptance_radius() { return _param_acceptance_radius.get(); } + float get_acceptance_radius(); int get_mavlink_fd() { return _mavlink_fd; } private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 295172ebb2..4bd7c08be0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -571,6 +571,20 @@ Navigator::publish_position_setpoint_triplet() } } +float +Navigator::get_acceptance_radius() +{ + float radius = _param_acceptance_radius.get(); + + if (hrt_elapsed_time(&_nav_caps.timestamp) < 1000000) { + if (_nav_caps.turn_distance > radius) { + radius = _nav_caps.turn_distance; + } + } + + return radius; +} + void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); From b2a694f71dcc246edd32c7b8befa0b3dd6f4879d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 10:34:17 +0200 Subject: [PATCH 43/51] navigator: Use the controller radius also as lower bound for mission items --- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/navigator.h | 15 +++++++++++++++ src/modules/navigator/navigator_main.cpp | 8 +++++++- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index dce7d45171..42c74428ad 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -123,12 +123,12 @@ MissionBlock::is_mission_item_reached() * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ - if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) { _waypoint_position_reached = true; } } else { /* for normal mission items used their acceptance radius */ - if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { _waypoint_position_reached = true; } } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 83f3487a00..95e3f9f964 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -144,7 +144,22 @@ public: Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } + + /** + * Get the acceptance radius + * + * @return the distance at which the next waypoint should be used + */ float get_acceptance_radius(); + + /** + * Get the acceptance radius given the mission item preset radius + * + * @param mission_item_radius the radius to use in case the controller-derived radius is smaller + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(float mission_item_radius); int get_mavlink_fd() { return _mavlink_fd; } private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4bd7c08be0..e05d079a0a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -574,7 +574,13 @@ Navigator::publish_position_setpoint_triplet() float Navigator::get_acceptance_radius() { - float radius = _param_acceptance_radius.get(); + return get_acceptance_radius(_param_acceptance_radius.get()); +} + +float +Navigator::get_acceptance_radius(float mission_item_radius) +{ + float radius = mission_item_radius; if (hrt_elapsed_time(&_nav_caps.timestamp) < 1000000) { if (_nav_caps.turn_distance > radius) { From 1fb743412810727d18f2815f541be1f75c11bfeb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 19:20:42 +0200 Subject: [PATCH 44/51] Navigation capabilites: Ensure regular publication of updated topic --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- src/modules/navigator/navigator_main.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b882cc08c4..350706f263 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1676,7 +1676,8 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + && turn_distance > 0)) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e05d079a0a..97baa1235f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -582,7 +582,7 @@ Navigator::get_acceptance_radius(float mission_item_radius) { float radius = mission_item_radius; - if (hrt_elapsed_time(&_nav_caps.timestamp) < 1000000) { + if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { if (_nav_caps.turn_distance > radius) { radius = _nav_caps.turn_distance; } From 1ecbf674aaffdbc6cfa7e3f30959283c9e8dba3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 7 Jun 2015 11:51:06 +0200 Subject: [PATCH 45/51] navigator: Finish rework of switch distance to account for vehicle dynamics --- src/modules/navigator/mission.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e36e3fa386..98b01c7578 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -586,7 +586,7 @@ Mission::altitude_sp_foh_update() } /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ - if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) { return; } @@ -608,7 +608,7 @@ Mission::altitude_sp_foh_update() /* if the minimal distance is smaller then 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 < _mission_item.acceptance_radius) { + if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { pos_sp_triplet->current.alt = _mission_item.altitude; } else { /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp @@ -617,7 +617,7 @@ Mission::altitude_sp_foh_update() * radius around the current waypoint **/ float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); - float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius)); float a = _mission_item_previous_alt - grad * _distance_current_previous; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; From f02ffa5a907b301cdaf21f5f5b4de7035f52888c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 7 Jun 2015 11:51:44 +0200 Subject: [PATCH 46/51] Att / Pos EKF: Fix handling of altitude initialization for local frame --- .../AttitudePositionEstimatorEKF.h | 3 +- .../ekf_att_pos_estimator_main.cpp | 42 +++++++++++-------- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 4d5e56a961..6a9e3aadbc 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -174,7 +174,7 @@ private: struct map_projection_reference_s _pos_ref; - float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; @@ -193,7 +193,6 @@ private: bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; - float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f3fe048e78..46f278b7d4 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -153,7 +153,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _sensor_combined {}, _pos_ref{}, - _baro_ref_offset(0.0f), + _filter_ref_offset(0.0f), _baro_gps_offset(0.0f), /* performance counters */ @@ -173,7 +173,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _gpsIsGood(false), _previousGPSTimestamp(0), _baro_init(false), - _baroAltRef(0.0f), _gps_initialized(false), _filter_start_time(0), _last_sensor_timestamp(0), @@ -404,6 +403,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Count the reset condition perf_count(_perf_reset); + _filter_ref_offset = _ekf->states[9]; } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -585,6 +585,7 @@ void AttitudePositionEstimatorEKF::task_main() _baro_init = false; _gps_initialized = false; + _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; @@ -643,12 +644,13 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->posNE[1] = 0.0f; _local_pos.ref_alt = 0.0f; - _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + _filter_ref_offset = -_baro.altitude; + } else { if (!_gps_initialized && _gpsIsGood) { @@ -711,7 +713,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame + _filter_ref_offset = _ekf->states[9]; // this should become zero in the local frame // init filtered gps and baro altitudes _gps_alt_filt = gps_alt; @@ -750,7 +752,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, - (double)_baro_ref_offset); + (double)_filter_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif @@ -811,7 +813,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef; + _local_pos.z = _ekf->states[9] - _filter_ref_offset; + //_local_pos.z_stable = _ekf->states[9]; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -859,7 +862,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; + _global_pos.alt = (-_local_pos.z) - _filter_ref_offset - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1070,8 +1073,9 @@ void AttitudePositionEstimatorEKF::print_status() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); - printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, + printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt); + printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt); + printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); @@ -1400,9 +1404,9 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; - if(!_baro_init) { + if (!_baro_init) { _baro_init = true; - _baroAltRef = _baro.altitude; + _baro_alt_filt = _baro.altitude; } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); @@ -1494,30 +1498,34 @@ int AttitudePositionEstimatorEKF::trip_nan() float nan_val = 0.0f / 0.0f; - warnx("system not armed, tripping state vector with NaN values"); + warnx("system not armed, tripping state vector with NaN"); _ekf->states[5] = nan_val; usleep(100000); - warnx("tripping covariance #1 with NaN values"); + warnx("tripping covariance #1 with NaN"); _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #2 with NaN values"); + warnx("tripping covariance #2 with NaN"); _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #3 with NaN values"); + warnx("tripping covariance #3 with NaN"); _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); - warnx("tripping Kalman gains with NaN values"); + warnx("tripping Kalman gains with NaN"); _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); - warnx("tripping stored states[0] with NaN values"); + warnx("tripping stored states[0] with NaN"); _ekf->storedStates[0][0] = nan_val; usleep(100000); + warnx("tripping states[9] with NaN"); + _ekf->states[9] = nan_val; + usleep(100000); + warnx("\nDONE - FILTER STATE:"); print_status(); } From e1ecac078d6f999722a1210ee983e2e77c8b1590 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 11:23:18 +0200 Subject: [PATCH 47/51] EKF: Harden GPS offset filter value for HIL --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 46f278b7d4..d408ea2ddb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1321,7 +1321,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); // update LPF - _gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + float filter_step += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + + if (isfinite(filter_step)) { + _gps_alt_filt += filter_step; + } } //check if we had a GPS outage for a long time From fe09e53b5bb7fe9f7d9734b23f3fa96145139d44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 14:12:12 +0200 Subject: [PATCH 48/51] EKF reset handling: Ensure altitude reinitializes correctly --- .../AttitudePositionEstimatorEKF.h | 6 ++ .../ekf_att_pos_estimator_main.cpp | 63 ++++++++++++++----- 2 files changed, 52 insertions(+), 17 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 6a9e3aadbc..0dd2b72d92 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -332,6 +332,12 @@ private: **/ void initializeGPS(); + /** + * Initialize the reference position for the local coordinate frame + */ + void initReferencePosition(hrt_abstime timestamp, + double lat, double lon, float gps_alt, float baro_alt); + /** * @brief * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d408ea2ddb..3ca47ebd49 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -401,9 +401,20 @@ int AttitudePositionEstimatorEKF::check_filter_state() // If error flag is set, we got a filter reset if (check && ekf_report.error) { + print_status(); + // Count the reset condition perf_count(_perf_reset); - _filter_ref_offset = _ekf->states[9]; + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + warnx("FILTER RESET"); + initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -649,7 +660,7 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - _filter_ref_offset = -_baro.altitude; + warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); } else { @@ -704,6 +715,32 @@ void AttitudePositionEstimatorEKF::task_main() _exit(0); } +void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, + double lat, double lon, float gps_alt, float baro_alt) +{ + // Reference altitude + if (isfinite(_ekf->states[9])) { + _filter_ref_offset = _ekf->states[9]; + } else if (isfinite(-_ekf->hgtMea)) { + _filter_ref_offset = -_ekf->hgtMea; + } else { + _filter_ref_offset = -_baro.altitude; + } + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _baro_alt_filt = baro_alt; + + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = timestamp; + + map_projection_init(&_pos_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); +} + void AttitudePositionEstimatorEKF::initializeGPS() { // GPS is in scaled integers, convert @@ -713,11 +750,6 @@ void AttitudePositionEstimatorEKF::initializeGPS() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _filter_ref_offset = _ekf->states[9]; // this should become zero in the local frame - - // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; - _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = _ekf->baroHgt; @@ -739,14 +771,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = _gps.timestamp_position; - - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, @@ -1321,7 +1346,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); // update LPF - float filter_step += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); if (isfinite(filter_step)) { _gps_alt_filt += filter_step; @@ -1416,7 +1441,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + + if (isfinite(filter_step)) { + _baro_alt_filt += filter_step; + } perf_count(_perf_baro); } From 3eebd8eb30a0956b9c7ce1d8fc6ddfb81141d4e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 14:28:19 +0200 Subject: [PATCH 49/51] EKF: Prevent bad data from being published --- .../ekf_att_pos_estimator_main.cpp | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3ca47ebd49..451e14d202 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -401,8 +401,6 @@ int AttitudePositionEstimatorEKF::check_filter_state() // If error flag is set, we got a filter reset if (check && ekf_report.error) { - print_status(); - // Count the reset condition perf_count(_perf_reset); // GPS is in scaled integers, convert @@ -413,7 +411,6 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - warnx("FILTER RESET"); initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { @@ -854,6 +851,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; + if (!isfinite(_local_pos.x) || + !isfinite(_local_pos.y) || + !isfinite(_local_pos.z) || + !isfinite(_local_pos.vx) || + !isfinite(_local_pos.vy) || + !isfinite(_local_pos.vz)) + { + // bad data, abort publication + return; + } + /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { /* publish the attitude setpoint */ @@ -902,6 +910,17 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; + if (!isfinite(_global_pos.lat) || + !isfinite(_global_pos.lon) || + !isfinite(_global_pos.alt) || + !isfinite(_global_pos.vel_n) || + !isfinite(_global_pos.vel_e) || + !isfinite(_global_pos.vel_d)) + { + // bad data, abort publication + return; + } + /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ From 0aa47236bfee600cf40fa3936bfec7ffb4ff104b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 16:48:42 +0200 Subject: [PATCH 50/51] commander: Only print reject mode message every 10 seconds. Set home position also if armed via command. Warn that arming via shell does not set home position. --- src/modules/commander/commander.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7b92942b33..d524aab787 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -143,7 +143,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 -#define PRINT_MODE_REJECT_INTERVAL 2000000 +#define PRINT_MODE_REJECT_INTERVAL 10000000 + +#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ @@ -347,6 +349,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); + warnx("note: not updating home position on commandline arming!"); close(mavlink_fd_local); exit(0); } @@ -478,6 +481,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + if (arming_ret == TRANSITION_CHANGED && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + commander_set_home_position(home_pub, _home, local_position, global_position); + } + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { @@ -2023,7 +2031,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { commander_set_home_position(home_pub, _home, local_position, global_position); } From 900c81e67cea119e7e145a2ad2f680643853e475 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Jun 2015 17:18:25 +0200 Subject: [PATCH 51/51] commander: Compile fix for home init on arming via CMD --- src/modules/commander/commander.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d524aab787..64fd972ba4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -171,6 +171,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ +static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; /* To remember when last notification was sent */ @@ -210,7 +211,7 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -453,7 +454,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct home_position_s *home, struct vehicle_global_position_s *global_pos, + struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -482,8 +484,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - if (arming_ret == TRANSITION_CHANGED && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { - commander_set_home_position(home_pub, _home, local_position, global_position); + if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); } if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { @@ -1169,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[]) set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } - const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); + commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1965,7 +1967,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) { status_changed = true; } }